Finally bring in completely revamped gyro code.
The gyro code should now be in its final state for the third robot.
Conflicts:
bot3/input/gyro_reader.cc
frc971/atom_code/atom_code.gyp
frc971/atom_code/build.sh
frc971/crio/build.sh
frc971/input/input.gyp
diff --git a/aos/atom_code/atom_code.gyp b/aos/atom_code/atom_code.gyp
index 37caccd..51dcaec 100644
--- a/aos/atom_code/atom_code.gyp
+++ b/aos/atom_code/atom_code.gyp
@@ -7,7 +7,7 @@
'<(AOS)/atom_code/init.cc',
],
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:shared_mem',
'<(AOS)/common/common.gyp:die',
'<(AOS)/build/aos.gyp:logging',
],
diff --git a/aos/atom_code/camera/Buffers.cpp b/aos/atom_code/camera/Buffers.cpp
index f580f89..9cd1a7d 100644
--- a/aos/atom_code/camera/Buffers.cpp
+++ b/aos/atom_code/camera/Buffers.cpp
@@ -15,7 +15,6 @@
};
const std::string Buffers::kFDServerName("/tmp/aos_fd_server");
const std::string Buffers::kQueueName("CameraBufferQueue");
-const aos_type_sig Buffers::kSignature{sizeof(Message), 971, 1};
int Buffers::CreateSocket(int (*bind_connect)(int, const sockaddr *, socklen_t)) {
union af_unix_sockaddr {
@@ -66,7 +65,7 @@
void Buffers::Release() {
if (message_ != NULL) {
- aos_queue_free_msg(queue_, message_);
+ queue_->FreeMessage(message_);
message_ = NULL;
}
}
@@ -77,11 +76,12 @@
// TODO(brians) make sure the camera reader process hasn't died
do {
if (block) {
- message_ = static_cast<const Message *>(aos_queue_read_msg(queue_, PEEK | BLOCK));
+ message_ = static_cast<const Message *>(queue_->ReadMessage(
+ RawQueue::kPeek | RawQueue::kBlock));
} else {
static int index = 0;
- message_ = static_cast<const Message *>(aos_queue_read_msg_index(queue_, BLOCK,
- &index));
+ message_ = static_cast<const Message *>(queue_->ReadMessageIndex(
+ RawQueue::kBlock, &index));
}
} while (block && message_ == NULL);
if (message_ != NULL) {
@@ -137,7 +137,7 @@
}
Buffers::Buffers() : server_(CreateSocket(connect)), fd_(FetchFD()), message_(NULL) {
MMap();
- queue_ = aos_fetch_queue(kQueueName.c_str(), &kSignature);
+ queue_ = RawQueue::Fetch(kQueueName.c_str(), sizeof(Message), 971, 1);
}
Buffers::~Buffers() {
@@ -157,6 +157,5 @@
}
}
-} // namespace camera
-} // namespace aos
-
+} // namespace camera
+} // namespace aos
diff --git a/aos/atom_code/camera/Buffers.h b/aos/atom_code/camera/Buffers.h
index 6b54188..080e9eb 100644
--- a/aos/atom_code/camera/Buffers.h
+++ b/aos/atom_code/camera/Buffers.h
@@ -53,9 +53,8 @@
// The current one. Sometimes NULL.
const Message *message_;
static const std::string kQueueName;
- static const aos_type_sig kSignature;
// NULL for the Reader one.
- aos_queue *queue_;
+ RawQueue *queue_;
// Make the actual mmap calls.
// Called by Buffers() automatically.
void MMap();
diff --git a/aos/atom_code/camera/Reader.cpp b/aos/atom_code/camera/Reader.cpp
index 5f30cfe..95f6128 100644
--- a/aos/atom_code/camera/Reader.cpp
+++ b/aos/atom_code/camera/Reader.cpp
@@ -17,6 +17,7 @@
#include "aos/atom_code/camera/V4L2.h"
#include "aos/atom_code/camera/Buffers.h"
#include "aos/common/logging/logging.h"
+#include "aos/atom_code/ipc_lib/queue.h"
#define CLEAR(x) memset(&(x), 0, sizeof(x))
@@ -31,8 +32,7 @@
// the bound socket listening for fd requests
int server_fd_;
- static const aos_type_sig kRecycleSignature;
- aos_queue *queue_, *recycle_queue_;
+ RawQueue *queue_, *recycle_queue_;
// the number of buffers currently queued in v4l2
uint32_t queued_;
public:
@@ -52,10 +52,11 @@
dev_name, errno, strerror(errno));
}
- queue_ = aos_fetch_queue_recycle(Buffers::kQueueName.c_str(), &Buffers::kSignature,
- &kRecycleSignature, &recycle_queue_);
+ queue_ = RawQueue::Fetch(Buffers::kQueueName.c_str(),
+ sizeof(Buffers::Message), 971, 1,
+ 1, Buffers::kNumBuffers, &recycle_queue_);
// read off any existing recycled messages
- while (aos_queue_read_msg(recycle_queue_, NON_BLOCK) != NULL);
+ while (recycle_queue_->ReadMessage(RawQueue::kNonBlock) != NULL);
queued_ = 0;
InitServer();
@@ -140,10 +141,11 @@
read = static_cast<const Buffers::Message *>(
// we block waiting for one if we can't dequeue one without leaving
// the driver <= 2 (to be safe)
- aos_queue_read_msg(recycle_queue_, (queued_ <= 2) ? BLOCK : NON_BLOCK));
+ recycle_queue_->ReadMessage((queued_ <= 2) ?
+ RawQueue::kBlock : RawQueue::kNonBlock));
if (read != NULL) {
buf.index = read->index;
- aos_queue_free_msg(recycle_queue_, read);
+ recycle_queue_->FreeMessage(read);
QueueBuffer(&buf);
}
} while (read != NULL);
@@ -163,7 +165,7 @@
}
Buffers::Message *const msg = static_cast<Buffers::Message *>(
- aos_queue_get_msg(queue_));
+ queue_->GetMessage());
if (msg == NULL) {
LOG(WARNING,
"couldn't get a message to send buf #%" PRIu32 " from queue %p."
@@ -175,7 +177,7 @@
msg->bytesused = buf.bytesused;
memcpy(&msg->timestamp, &buf.timestamp, sizeof(msg->timestamp));
msg->sequence = buf.sequence;
- if (aos_queue_write_msg_free(queue_, msg, OVERRIDE) == -1) {
+ if (!queue_->WriteMessage(msg, RawQueue::kOverride)) {
LOG(WARNING,
"sending message %p with buf #%" PRIu32 " to queue %p failed."
" re-queueing now\n", msg, buf.index, queue_);
@@ -405,8 +407,6 @@
}
};
const char *const Reader::dev_name = "/dev/video0";
-const aos_type_sig Reader::kRecycleSignature{
- sizeof(Buffers::Message), 1, Buffers::kNumBuffers};
} // namespace camera
} // namespace aos
diff --git a/aos/atom_code/camera/camera.gyp b/aos/atom_code/camera/camera.gyp
index 9931806..f1743be 100644
--- a/aos/atom_code/camera/camera.gyp
+++ b/aos/atom_code/camera/camera.gyp
@@ -45,11 +45,11 @@
'Buffers.cpp',
],
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
'<(AOS)/build/aos.gyp:logging',
],
'export_dependent_settings': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
},
{
@@ -74,6 +74,7 @@
'buffers',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
},
],
diff --git a/aos/atom_code/core/BinaryLogReader.cpp b/aos/atom_code/core/BinaryLogReader.cpp
index 9451fd1..3e96cc5 100644
--- a/aos/atom_code/core/BinaryLogReader.cpp
+++ b/aos/atom_code/core/BinaryLogReader.cpp
@@ -97,7 +97,7 @@
output->sequence = msg->sequence;
memcpy(output_strings, msg->name, name_size);
memcpy(output_strings + name_size, msg->message, message_size);
- condition_set(&output->marker);
+ futex_set(&output->marker);
logging::atom::Free(msg);
}
diff --git a/aos/atom_code/core/LogFileCommon.h b/aos/atom_code/core/LogFileCommon.h
index afb86b0..3798b06 100644
--- a/aos/atom_code/core/LogFileCommon.h
+++ b/aos/atom_code/core/LogFileCommon.h
@@ -28,13 +28,13 @@
// A lot of the fields don't have comments because they're the same as the
// identically named fields in LogMessage.
struct __attribute__((aligned)) LogFileMessageHeader {
- // gets condition_set once this one has been written
+ // gets futex_set once this one has been written
// for readers keeping up with a live writer
//
// gets initialized to 0 by ftruncate
//
// there will be something here after the last log on a "page" set to 2
- // (by the condition_set) to indicate that the next log is on the next page
+ // (by the futex_set) to indicate that the next log is on the next page
mutex marker;
static_assert(sizeof(marker) == 4, "mutex changed size!");
log_level level;
@@ -132,8 +132,8 @@
sizeof(mutex) > kPageSize) {
char *const temp = current_;
MapNextPage();
- if (condition_set_value(reinterpret_cast<mutex *>(&temp[position_]), 2) == -1) {
- fprintf(stderr, "LogFileCommon: condition_set_value(%p, 2) failed with %d: %s."
+ if (futex_set_value(reinterpret_cast<mutex *>(&temp[position_]), 2) == -1) {
+ fprintf(stderr, "LogFileCommon: futex_set_value(%p, 2) failed with %d: %s."
" readers will hang\n", &temp[position_], errno, strerror(errno));
}
Unmap(temp);
@@ -152,7 +152,7 @@
do {
r = reinterpret_cast<LogFileMessageHeader *>(¤t_[position_]);
if (wait) {
- if (condition_wait(&r->marker) != 0) continue;
+ if (futex_wait(&r->marker) != 0) continue;
}
if (r->marker == 2) {
Unmap(current_);
diff --git a/aos/atom_code/core/LogStreamer.cpp b/aos/atom_code/core/LogStreamer.cpp
index d3d4928..20c5987 100644
--- a/aos/atom_code/core/LogStreamer.cpp
+++ b/aos/atom_code/core/LogStreamer.cpp
@@ -31,7 +31,7 @@
int index = 0;
while (true) {
- const LogMessage *const msg = ReadNext(BLOCK, &index);
+ const LogMessage *const msg = ReadNext(RawQueue::kBlock, &index);
if (msg == NULL) continue;
internal::PrintMessage(stdout, *msg);
diff --git a/aos/atom_code/core/core.gyp b/aos/atom_code/core/core.gyp
index 74b3232..1aaebfb 100644
--- a/aos/atom_code/core/core.gyp
+++ b/aos/atom_code/core/core.gyp
@@ -32,6 +32,7 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/atom_code/atom_code.gyp:init',
'<(AOS)/common/common.gyp:time',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
},
{
diff --git a/aos/atom_code/ipc_lib/aos_sync.c b/aos/atom_code/ipc_lib/aos_sync.c
index a9a4779..1980a4d 100644
--- a/aos/atom_code/ipc_lib/aos_sync.c
+++ b/aos/atom_code/ipc_lib/aos_sync.c
@@ -1,135 +1,197 @@
+#include "aos/atom_code/ipc_lib/aos_sync.h"
+
#include <stdio.h>
#include <linux/futex.h>
#include <unistd.h>
#include <sys/syscall.h>
#include <errno.h>
-#include "aos_sync.h"
-#include "cmpxchg.h"
#include <stdint.h>
#include <limits.h>
#include <string.h>
+#include <inttypes.h>
+
+#include "cmpxchg.h"
// this code is based on something that appears to be based on <http://www.akkadia.org/drepper/futex.pdf>, which also has a lot of useful information
// should probably use <http://lxr.linux.no/linux+v2.6.34/Documentation/robust-futexes.txt> once it becomes available
-// <http://locklessinc.com/articles/futex_cheat_sheet/> and <http://locklessinc.com/articles/mutex_cv_futex/> are useful
+// (sys_set_robust_list appears to be the function name)
+// <http://locklessinc.com/articles/futex_cheat_sheet/> and
+// <http://locklessinc.com/articles/mutex_cv_futex/> are useful
// <http://lwn.net/Articles/360699/> has a nice overview of futexes in late 2009 (fairly recent compared to everything else...)
// can't use PRIVATE futex operations because they use the pid (or something) as part of the hash
//
+// Remember that EAGAIN and EWOUDBLOCK are the same! (ie if you get EAGAIN from
+// FUTEX_WAIT, the docs call it EWOULDBLOCK...)
+//
// Values for a mutex:
// 0 = unlocked
// 1 = locked, not contended
// 2 = locked, probably contended
-// Values for a condition:
+// Values for a "futex":
// 0 = unset
// 1 = set
-static inline int sys_futex(mutex *addr1, int op, int val1, const struct timespec *timeout,
- void *addr2, int val3) {
- return syscall(SYS_futex, addr1, op, val1, timeout, addr2, val3);
+static inline int sys_futex(mutex *addr1, int op, int val1,
+ const struct timespec *timeout, void *addr2, int val3) {
+ return syscall(SYS_futex, addr1, op, val1, timeout, addr2, val3);
+}
+static inline int sys_futex_requeue(mutex *addr1, int op, int num_wake,
+ int num_requeue, mutex *m) {
+ return syscall(SYS_futex, addr1, op, num_wake, num_requeue, m);
+}
+static inline int sys_futex_op(mutex *addr1, int op, int num_waiters1,
+ int num_waiters2, mutex *addr2, int op_args_etc) {
+ return syscall(SYS_futex, addr1, op, num_waiters1,
+ num_waiters2, addr2, op_args_etc);
}
static inline int mutex_get(mutex *m, uint8_t signals_fail, const
- struct timespec *timeout) {
- int c;
- c = cmpxchg(m, 0, 1);
- if (!c) return 0;
- /* The lock is now contended */
- if (c == 1) c = xchg(m, 2);
- while (c) {
- /* Wait in the kernel */
- //printf("sync here %d\n", __LINE__);
- if (sys_futex(m, FUTEX_WAIT, 2, timeout, NULL, 0) == -1) {
- if (signals_fail && errno == EINTR) {
- return 1;
- }
- if (timeout != NULL && errno == ETIMEDOUT) {
- return 2;
- }
- }
- //printf("sync here %d\n", __LINE__);
- c = xchg(m, 2);
- }
- return 0;
+ struct timespec *timeout) {
+ int c;
+ c = cmpxchg(m, 0, 1);
+ if (!c) return 0;
+ /* The lock is now contended */
+ if (c == 1) c = xchg(m, 2);
+ while (c) {
+ /* Wait in the kernel */
+ //printf("sync here %d\n", __LINE__);
+ if (sys_futex(m, FUTEX_WAIT, 2, timeout, NULL, 0) == -1) {
+ if (signals_fail && errno == EINTR) {
+ return 1;
+ }
+ if (timeout != NULL && errno == ETIMEDOUT) {
+ return 2;
+ }
+ }
+ //printf("sync here %d\n", __LINE__);
+ c = xchg(m, 2);
+ }
+ return 0;
}
int mutex_lock(mutex *m) {
- return mutex_get(m, 1, NULL);
+ return mutex_get(m, 1, NULL);
}
int mutex_lock_timeout(mutex *m, const struct timespec *timeout) {
- return mutex_get(m, 1, timeout);
+ return mutex_get(m, 1, timeout);
}
int mutex_grab(mutex *m) {
- return mutex_get(m, 0, NULL);
+ return mutex_get(m, 0, NULL);
}
-int mutex_unlock(mutex *m) {
- /* Unlock, and if not contended then exit. */
- //printf("mutex_unlock(%p) => %d \n",m,*m);
- switch (xchg(m, 0)) {
- case 0:
- fprintf(stderr, "sync: multiple unlock of %p. aborting\n", m);
- printf("see stderr\n");
- abort();
- case 1:
- //printf("mutex_unlock return(%p) => %d \n",m,*m);
- return 0;
- case 2:
- if (sys_futex(m, FUTEX_WAKE, 1, NULL, NULL, 0) == -1) {
- fprintf(stderr, "sync: waking 1 from %p failed with %d: %s\n",
- m, errno, strerror(errno));
- return -1;
- } else {
- return 0;
- }
- default:
- fprintf(stderr, "sync: got a garbage value from mutex %p. aborting\n",
- m);
- printf("see stderr\n");
- abort();
- }
+void mutex_unlock(mutex *m) {
+ /* Unlock, and if not contended then exit. */
+ //printf("mutex_unlock(%p) => %d \n",m,*m);
+ switch (xchg(m, 0)) {
+ case 0:
+ fprintf(stderr, "sync: multiple unlock of %p. aborting\n", m);
+ printf("see stderr\n");
+ abort();
+ case 1:
+ //printf("mutex_unlock return(%p) => %d \n",m,*m);
+ break;
+ case 2:
+ if (sys_futex(m, FUTEX_WAKE, 1, NULL, NULL, 0) == -1) {
+ fprintf(stderr, "sync: waking 1 from %p failed with %d: %s\n",
+ m, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ } else {
+ break;
+ }
+ default:
+ fprintf(stderr, "sync: got a garbage value from mutex %p. aborting\n",
+ m);
+ printf("see stderr\n");
+ abort();
+ }
}
int mutex_trylock(mutex *m) {
- /* Try to take the lock, if is currently unlocked */
- unsigned c = cmpxchg(m, 0, 1);
- if (!c) return 0;
- return 1;
+ /* Try to take the lock, if is currently unlocked */
+ unsigned c = cmpxchg(m, 0, 1);
+ if (!c) return 0;
+ return 1;
}
-int condition_wait(mutex *m) {
- if (*m) {
- return 0;
- }
- if (sys_futex(m, FUTEX_WAIT, 0, NULL, NULL, 0) == -1) {
- if (errno == EINTR) {
- return 1;
- } else if (errno != EWOULDBLOCK) {
- return -1;
- }
- }
- return 0;
+int futex_wait(mutex *m) {
+ if (*m) {
+ return 0;
+ }
+ if (sys_futex(m, FUTEX_WAIT, 0, NULL, NULL, 0) == -1) {
+ if (errno == EINTR) {
+ return 1;
+ } else if (errno != EWOULDBLOCK) {
+ return -1;
+ }
+ }
+ return 0;
}
-int condition_wait_force(mutex *m) {
- while (1) {
- if (sys_futex(m, FUTEX_WAIT, *m, NULL, NULL, 0) == -1) {
- if (errno != EWOULDBLOCK) { // if it was an actual problem
- if (errno == EINTR) {
- return 1;
- } else {
- return -1;
- }
- }
- } else {
- return 0;
- }
- }
+int futex_set_value(mutex *m, mutex value) {
+ xchg(m, value);
+ return sys_futex(m, FUTEX_WAKE, INT_MAX, NULL, NULL, 0);
}
-inline int condition_set_value(mutex *m, mutex value) {
- xchg(m, value);
- return sys_futex(m, FUTEX_WAKE, INT_MAX, NULL, NULL, 0);
+int futex_set(mutex *m) {
+ return futex_set_value(m, 1);
}
-int condition_set(mutex *m) {
- return condition_set_value(m, 1);
-}
-int condition_unset(mutex *m) {
- return !xchg(m, 0);
+int futex_unset(mutex *m) {
+ return !xchg(m, 0);
}
+void condition_wait(mutex *c, mutex *m) {
+ const mutex wait_start = *c;
+
+ mutex_unlock(m);
+
+ while (1) {
+ if (sys_futex(c, FUTEX_WAIT, wait_start, NULL, NULL, 0) == -1) {
+ // If it failed for some reason other than somebody else doing a wake
+ // before we actually made it to sleep.
+ if (__builtin_expect(*c == wait_start, 0)) {
+ // Try again if it was because of a signal.
+ if (errno == EINTR) continue;
+ fprintf(stderr, "FUTEX_WAIT(%p, %"PRIu32", NULL, NULL, 0) failed"
+ " with %d: %s\n",
+ c, wait_start, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+ }
+ // Simplified mutex_lock that always leaves it
+ // contended in case anybody else got requeued.
+ while (xchg(m, 2) != 0) {
+ if (sys_futex(m, FUTEX_WAIT, 2, NULL, NULL, 0) == -1) {
+ // Try again if it was because of a signal or somebody else unlocked it
+ // before we went to sleep.
+ if (errno == EINTR || errno == EWOULDBLOCK) continue;
+ fprintf(stderr, "sync: FUTEX_WAIT(%p, 2, NULL, NULL, 0)"
+ " failed with %d: %s\n",
+ m, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+ }
+ return;
+ }
+}
+
+void condition_signal(mutex *c) {
+ __sync_fetch_and_add(c, 1);
+ if (sys_futex(c, FUTEX_WAKE, 1, NULL, NULL, 0) == -1) {
+ fprintf(stderr, "sync: FUTEX_WAKE(%p, 1, NULL, NULL, 0)"
+ " failed with %d: %s\n",
+ c, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+}
+
+void condition_broadcast(mutex *c, mutex *m) {
+ __sync_fetch_and_add(c, 1);
+ // Wake 1 waiter and requeue the rest.
+ if (sys_futex_requeue(c, FUTEX_REQUEUE, 1, INT_MAX, m) == -1) {
+ fprintf(stderr, "sync: FUTEX_REQUEUE(%p, 1, INT_MAX, %p, 0)"
+ " failed with %d: %s\n",
+ c, m, errno, strerror(errno));
+ printf("see stderr\n");
+ abort();
+ }
+}
diff --git a/aos/atom_code/ipc_lib/aos_sync.h b/aos/atom_code/ipc_lib/aos_sync.h
index 3c66264..6d4bf55 100644
--- a/aos/atom_code/ipc_lib/aos_sync.h
+++ b/aos/atom_code/ipc_lib/aos_sync.h
@@ -1,5 +1,5 @@
-#ifndef AOS_IPC_LIB_SYNC_H_
-#define AOS_IPC_LIB_SYNC_H_
+#ifndef AOS_ATOM_CODE_IPC_LIB_SYNC_H_
+#define AOS_ATOM_CODE_IPC_LIB_SYNC_H_
#include <stdlib.h>
#include <signal.h>
@@ -14,50 +14,83 @@
// and <http://www.valgrind.org/docs/manual/drd-manual.html#drd-manual.clientreqs>
// list the interesting ones
-// Have to align structs containing it to to sizeof(int).
+// Have to align structs containing it to sizeof(int).
// Valid initial values for use with mutex_ functions are 0 (unlocked) and 1 (locked).
-// Valid initial values for use with condition_ functions are 0 (unset) and 1 (set).
+// Valid initial values for use with futex_ functions are 0 (unset) and 1 (set).
+// No initialization is necessary for use as c with the condition_ functions.
// The value should not be changed after multiple processes have started
// accessing an instance except through the functions declared in this file.
typedef volatile uint32_t mutex __attribute__((aligned(sizeof(int))));
// All return -1 for other error (which will be in errno from futex(2)).
+//
+// There is no priority inversion protection.
+// TODO(brians) look at using
+// <http://www.kernel.org/doc/Documentation/pi-futex.txt>
// Returns 1 if interrupted by a signal.
+//
+// One of the highest priority processes blocked on a given mutex will be the
+// one to lock it when it is unlocked.
int mutex_lock(mutex *m) __attribute__((warn_unused_result));
// Returns 2 if it timed out or 1 if interrupted by a signal.
int mutex_lock_timeout(mutex *m, const struct timespec *timeout)
__attribute__((warn_unused_result));
// Ignores signals. Can not fail.
int mutex_grab(mutex *m);
-// Returns 1 for multiple unlocking and -1 if something bad happened and
-// whoever's waiting didn't get woken up.
-int mutex_unlock(mutex *m);
+// abort(2)s for multiple unlocking.
+void mutex_unlock(mutex *m);
// Returns 0 when successful in locking the mutex and 1 if somebody else has it
// locked.
int mutex_trylock(mutex *m) __attribute__((warn_unused_result));
-// The condition_ functions are similar to the mutex_ ones but different.
+// The futex_ functions are similar to the mutex_ ones but different.
// They are designed for signalling when something happens (possibly to
// multiple listeners). A mutex manipulated with them can only be set or unset.
+//
+// They are different from the condition_ functions in that they do NOT work
+// correctly as standard condition variables. While it is possible to keep
+// track of the "condition" using the value part of the futex_* functions, the
+// obvious implementation has basically the same race condition that condition
+// variables are designed to prevent between somebody else grabbing the mutex
+// and changing whether it's set or not and the futex_ function changing the
+// futex's value.
-// Wait for the condition to be set. Will return immediately if it's already set.
-// Returns 0 if successful or it was already set, 1 if interrupted by a signal, or -1.
-int condition_wait(mutex *m) __attribute__((warn_unused_result));
-// Will wait for the next condition_set, even if the condition is already set.
-// Returns 0 if successful, 1 if interrupted by a signal, or -1.
-int condition_wait_force(mutex *m) __attribute__((warn_unused_result));
-// Set the condition and wake up anybody waiting on it.
+// Wait for the futex to be set. Will return immediately if it's already set.
+// Returns 0 if successful or it was already set, 1 if interrupted by a signal,
+// or -1.
+int futex_wait(mutex *m) __attribute__((warn_unused_result));
+// Set the futex and wake up anybody waiting on it.
// Returns the number that were woken or -1.
-int condition_set(mutex *m);
+//
+// This will always wake up all waiters at the same time and set the value to 1.
+int futex_set(mutex *m);
// Same as above except lets something other than 1 be used as the final value.
-int condition_set_value(mutex *m, mutex value);
-// Unsets the condition.
+int futex_set_value(mutex *m, mutex value);
+// Unsets the futex (sets the value to 0).
// Returns 0 if it was set before and 1 if it wasn't.
-int condition_unset(mutex *m);
+// Can not fail.
+int futex_unset(mutex *m);
+
+// The condition_ functions implement condition variable support. The API is
+// similar to the pthreads api and works the same way. The same m argument must
+// be passed in for all calls to all of the condition_ functions with a given c.
+
+// Wait for the condition variable to be signalled. m will be unlocked
+// atomically with actually starting to wait. m is guaranteed to be locked when
+// this function returns.
+// NOTE: The relocking of m is not atomic with stopping the actual wait and
+// other process(es) may lock (+unlock) the mutex first.
+void condition_wait(mutex *c, mutex *m);
+// If any other processes are condition_waiting on c, wake 1 of them. Does not
+// require m to be locked.
+void condition_signal(mutex *c);
+// Wakes all processes that are condition_waiting on c. Does not require m to be
+// locked.
+void condition_broadcast(mutex *c, mutex *m);
#ifdef __cplusplus
}
#endif // __cplusplus
-#endif
+#endif // AOS_ATOM_CODE_IPC_LIB_SYNC_H_
diff --git a/aos/atom_code/ipc_lib/binheap.c b/aos/atom_code/ipc_lib/binheap.c
deleted file mode 100644
index 8eb024d..0000000
--- a/aos/atom_code/ipc_lib/binheap.c
+++ /dev/null
@@ -1,125 +0,0 @@
-#include "binheap.h"
-#include <stdlib.h>
-#include <stdio.h>
-
-#ifndef TESTING_ASSERT
-#define TESTING_ASSERT(...)
-#endif
-#define Error(x) TESTING_ASSERT(0, x)
-
-#define MinData (0)
-
-void Initialize( int Elements, PriorityQueue H )
-{
- H->Capacity = Elements - 1;
- H->Size = 0;
- H->Elements[ 0 ] = MinData;
-}
-
-int Insert( ElementType X, PriorityQueue H )
-{
- int i;
-
- if( IsFull( H ) )
- {
- return -1;
- }
-
- for( i = ++H->Size; H->Elements[ i / 2 ] > X; i /= 2 )
- H->Elements[ i ] = H->Elements[ i / 2 ];
- H->Elements[ i ] = X;
- return 0;
-}
-
-void Remove( ElementType X, PriorityQueue H )
-{
- int i, Child, removed = 0;
- ElementType LastElement;
-
- for ( i = 1; i <= H->Size; ++i )
- {
- if( H->Elements[ i ] == X )
- {
- removed = i;
- break;
- }
- }
- if( removed == 0 )
- {
- fprintf(stderr, "could not find element %d to remove. not removing any\n", X);
- return;
- }
-
- LastElement = H->Elements[ H->Size-- ];
-
- for( i = removed; i * 2 <= H->Size; i = Child )
- {
- /* Find smaller child */
- Child = i * 2;
- if( Child != H->Size && H->Elements[ Child + 1 ]
- < H->Elements[ Child ] )
- Child++;
-
- /* Percolate one level */
- if( LastElement > H->Elements[ Child ] )
- H->Elements[ i ] = H->Elements[ Child ];
- else
- break;
- }
- H->Elements[ i ] = LastElement;
-}
-
-ElementType DeleteMin( PriorityQueue H )
-{
- int i, Child;
- ElementType MinElement, LastElement;
-
- if( IsEmpty( H ) )
- {
- Error( "Priority queue is empty" );
- return H->Elements[ 0 ];
- }
- MinElement = H->Elements[ 1 ];
- LastElement = H->Elements[ H->Size-- ];
-
- for( i = 1; i * 2 <= H->Size; i = Child )
- {
- /* Find smaller child */
- Child = i * 2;
- if( Child != H->Size && H->Elements[ Child + 1 ]
- < H->Elements[ Child ] )
- Child++;
-
- /* Percolate one level */
- if( LastElement > H->Elements[ Child ] )
- H->Elements[ i ] = H->Elements[ Child ];
- else
- break;
- }
- H->Elements[ i ] = LastElement;
- return MinElement;
-}
-
-ElementType GetMin( PriorityQueue H )
-{
- if( !IsEmpty( H ) )
- return H->Elements[ 1 ];
- Error( "Priority Queue is Empty" );
- return H->Elements[ 0 ];
-}
-
-int IsEmpty( PriorityQueue H )
-{
- return H->Size == 0;
-}
-
-int IsFull( PriorityQueue H )
-{
- return H->Size == H->Capacity;
-}
-
-int GetSize( PriorityQueue H )
-{
- return H->Size;
-}
-
diff --git a/aos/atom_code/ipc_lib/binheap.h b/aos/atom_code/ipc_lib/binheap.h
deleted file mode 100644
index 8c26f9f..0000000
--- a/aos/atom_code/ipc_lib/binheap.h
+++ /dev/null
@@ -1,29 +0,0 @@
-#ifndef _BinHeap_H
-#define _BinHeap_H
-
-#include <stdint.h>
-
-typedef uint8_t ElementType;
-struct HeapStruct;
-typedef struct HeapStruct *PriorityQueue;
-
-struct HeapStruct
-{
- int Capacity;
- int Size;
- ElementType *Elements;
-};
-
-// Elements is the number allocated at H->Elements
-void Initialize( int Elements, PriorityQueue H );
-// 0 if successful, -1 if full
-int Insert( ElementType X, PriorityQueue H );
-void Remove( ElementType X, PriorityQueue H );
-ElementType DeleteMin( PriorityQueue H );
-ElementType GetMin( PriorityQueue H );
-int IsEmpty( PriorityQueue H );
-int IsFull( PriorityQueue H );
-int GetSize( PriorityQueue H );
-
-#endif
-
diff --git a/aos/atom_code/ipc_lib/binheap_test.cpp b/aos/atom_code/ipc_lib/binheap_test.cpp
deleted file mode 100644
index 62eecd4..0000000
--- a/aos/atom_code/ipc_lib/binheap_test.cpp
+++ /dev/null
@@ -1,65 +0,0 @@
-extern "C" {
-#include "binheap.h"
-}
-
-#include <gtest/gtest.h>
-
-class BinHeapTest : public testing::Test{
- protected:
- static const int TEST_ELEMENTS = 57;
- PriorityQueue queue;
- virtual void SetUp(){
- queue = new HeapStruct();
- queue->Elements = new uint8_t[TEST_ELEMENTS];
- Initialize(TEST_ELEMENTS, queue);
- }
- virtual void TearDown(){
- delete[] queue->Elements;
- delete queue;
- }
-};
-
-std::ostream& operator<< (std::ostream& o, uint8_t c){
- return o<<(int)c;
-}
-
-testing::AssertionResult Contains(PriorityQueue queue, const uint8_t expected[], size_t length){
- for(size_t i = 0; i < length; ++i){
- //printf("expected[%d]=%d\n", i, expected[i]);
- if(DeleteMin(queue) != expected[i]){
- return testing::AssertionFailure() << "queue[" << i << "] != " << expected[i];
- }
- }
- if(!IsEmpty(queue))
- return testing::AssertionFailure() << "queue is longer than " << length;
- return ::testing::AssertionSuccess();
-}
-
-TEST_F(BinHeapTest, SingleElement){
- Insert(87, queue);
- EXPECT_EQ(87, DeleteMin(queue));
- EXPECT_TRUE(IsEmpty(queue));
-}
-TEST_F(BinHeapTest, MultipleElements){
- Insert(54, queue);
- Insert(1, queue);
- Insert(0, queue);
- Insert(255, queue);
- Insert(123, queue);
- uint8_t expected[] = {0, 1, 54, 123, 255};
- EXPECT_TRUE(Contains(queue, expected, sizeof(expected)));
-}
-TEST_F(BinHeapTest, Removals){
- Insert(54, queue);
- Insert(1, queue);
- Insert(0, queue);
- Insert(255, queue);
- Insert(123, queue);
- Remove(255, queue);
- Remove(0, queue);
- Insert(222, queue);
- Insert(67, queue);
- uint8_t expected[] = {1, 54, 67, 123, 222};
- EXPECT_TRUE(Contains(queue, expected, sizeof(expected)));
-}
-
diff --git a/aos/atom_code/ipc_lib/cmpxchg.h b/aos/atom_code/ipc_lib/cmpxchg.h
index acb4a3c..715c57d 100644
--- a/aos/atom_code/ipc_lib/cmpxchg.h
+++ b/aos/atom_code/ipc_lib/cmpxchg.h
@@ -9,10 +9,10 @@
#define cmpxchg(ptr, o, n) __sync_val_compare_and_swap(ptr, o, n)
/*#define xchg(ptr, n) ({typeof(*ptr) r; \
- do{ \
- r = *ptr; \
- }while(!__sync_bool_compare_and_swap(ptr, r, n)); \
- r; \
+ do{ \
+ r = *ptr; \
+ }while(!__sync_bool_compare_and_swap(ptr, r, n)); \
+ r; \
})*/
# define LOCK "lock;"
@@ -24,7 +24,7 @@
/*static inline void set_64bit(volatile unsigned long *ptr, unsigned long val)
{
- *ptr = val;
+ *ptr = val;
}
#define _set_64bit set_64bit*/
@@ -32,37 +32,37 @@
/*
* Note: no "lock" prefix even on SMP: xchg always implies lock anyway
* Note 2: xchg has side effect, so that attribute volatile is necessary,
- * but generally the primitive is invalid, *ptr is output argument. --ANK
+ * but generally the primitive is invalid, *ptr is output argument. --ANK
*/
static inline unsigned long __xchg(unsigned long x, volatile void * ptr, int size)
{
- switch (size) {
- case 1:
- __asm__ __volatile__("xchgb %b0,%1"
- :"=q" (x)
- :"m" (*__xg(ptr)), "0" (x)
- :"memory");
- break;
- case 2:
- __asm__ __volatile__("xchgw %w0,%1"
- :"=r" (x)
- :"m" (*__xg(ptr)), "0" (x)
- :"memory");
- break;
- case 4:
- __asm__ __volatile__("xchgl %k0,%1"
- :"=r" (x)
- :"m" (*__xg(ptr)), "0" (x)
- :"memory");
- break;
- case 8:
- __asm__ __volatile__("xchg %0,%1"
- :"=r" (x)
- :"m" (*__xg(ptr)), "0" (x)
- :"memory");
- break;
- }
- return x;
+ switch (size) {
+ case 1:
+ __asm__ __volatile__("xchgb %b0,%1"
+ :"=q" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ case 2:
+ __asm__ __volatile__("xchgw %w0,%1"
+ :"=r" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ case 4:
+ __asm__ __volatile__("xchgl %k0,%1"
+ :"=r" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ case 8:
+ __asm__ __volatile__("xchg %0,%1"
+ :"=r" (x)
+ :"m" (*__xg(ptr)), "0" (x)
+ :"memory");
+ break;
+ }
+ return x;
}
/*
@@ -76,78 +76,78 @@
#define __HAVE_ARCH_CMPXCHG 1
static inline unsigned long __cmpxchg(volatile void *ptr, unsigned long old,
- unsigned long new, int size)
+ unsigned long new, int size)
{
- int32_t prev;
- switch (size) {
- case 1:
- __asm__ __volatile__(LOCK_PREFIX "cmpxchgb %b1,%2"
- : "=a"(prev)
- : "q"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- case 2:
- __asm__ __volatile__(LOCK_PREFIX "cmpxchgw %w1,%2"
- : "=a"(prev)
- : "r"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- case 4:
- __asm__ __volatile__(LOCK_PREFIX "cmpxchgl %k1,%2"
- : "=a"(prev)
- : "r"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- case 8:
- __asm__ __volatile__("lock; cmpxchg %1,%2"
- : "=a"(prev)
- : "q"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- }
- return old;
+ int32_t prev;
+ switch (size) {
+ case 1:
+ __asm__ __volatile__(LOCK_PREFIX "cmpxchgb %b1,%2"
+ : "=a"(prev)
+ : "q"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 2:
+ __asm__ __volatile__(LOCK_PREFIX "cmpxchgw %w1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 4:
+ __asm__ __volatile__(LOCK_PREFIX "cmpxchgl %k1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 8:
+ __asm__ __volatile__("lock; cmpxchg %1,%2"
+ : "=a"(prev)
+ : "q"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ }
+ return old;
}
/*
static inline unsigned long __cmpxchg_local(volatile void *ptr,
- unsigned long old, unsigned long new, int size)
+ unsigned long old, unsigned long new, int size)
{
- unsigned long prev;
- switch (size) {
- case 1:
- __asm__ __volatile__("cmpxchgb %b1,%2"
- : "=a"(prev)
- : "q"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- case 2:
- __asm__ __volatile__("cmpxchgw %w1,%2"
- : "=a"(prev)
- : "r"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- case 4:
- __asm__ __volatile__("cmpxchgl %k1,%2"
- : "=a"(prev)
- : "r"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- case 8:
- __asm__ __volatile__("cmpxchgq %1,%2"
- : "=a"(prev)
- : "r"(new), "m"(*__xg(ptr)), "0"(old)
- : "memory");
- return prev;
- }
- return old;
+ unsigned long prev;
+ switch (size) {
+ case 1:
+ __asm__ __volatile__("cmpxchgb %b1,%2"
+ : "=a"(prev)
+ : "q"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 2:
+ __asm__ __volatile__("cmpxchgw %w1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 4:
+ __asm__ __volatile__("cmpxchgl %k1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ case 8:
+ __asm__ __volatile__("cmpxchgq %1,%2"
+ : "=a"(prev)
+ : "r"(new), "m"(*__xg(ptr)), "0"(old)
+ : "memory");
+ return prev;
+ }
+ return old;
}*/
#define cmpxchg(ptr,o,n)\
- ((__typeof__(*(ptr)))__cmpxchg((ptr),(unsigned long)(o),\
- (unsigned long)(n),sizeof(*(ptr))))
+ ((__typeof__(*(ptr)))__cmpxchg((ptr),(unsigned long)(o),\
+ (unsigned long)(n),sizeof(*(ptr))))
/*#define cmpxchg_local(ptr,o,n)\
- ((__typeof__(*(ptr)))__cmpxchg((ptr),(unsigned long)(o),\
- (unsigned long)(n),sizeof(*(ptr))))*/
+ ((__typeof__(*(ptr)))__cmpxchg((ptr),(unsigned long)(o),\
+ (unsigned long)(n),sizeof(*(ptr))))*/
#endif
#endif
diff --git a/aos/atom_code/ipc_lib/condition.cc b/aos/atom_code/ipc_lib/condition.cc
new file mode 100644
index 0000000..b764026
--- /dev/null
+++ b/aos/atom_code/ipc_lib/condition.cc
@@ -0,0 +1,26 @@
+#include "aos/common/condition.h"
+
+#include <inttypes.h>
+
+#include "aos/common/type_traits.h"
+
+namespace aos {
+
+static_assert(shm_ok<Condition>::value, "Condition should work"
+ " in shared memory");
+
+Condition::Condition(Mutex *m) : impl_(), m_(m) {}
+
+void Condition::Wait() {
+ condition_wait(&impl_, &m_->impl_);
+}
+
+void Condition::Signal() {
+ condition_signal(&impl_);
+}
+
+void Condition::Broadcast() {
+ condition_broadcast(&impl_, &m_->impl_);
+}
+
+} // namespace aos
diff --git a/aos/atom_code/ipc_lib/core_lib.c b/aos/atom_code/ipc_lib/core_lib.c
index 53587d8..bbd2f5b 100644
--- a/aos/atom_code/ipc_lib/core_lib.c
+++ b/aos/atom_code/ipc_lib/core_lib.c
@@ -1,49 +1,43 @@
+#include "aos/atom_code/ipc_lib/core_lib.h"
+
#include <stdio.h>
#include <stdlib.h>
-#include "shared_mem.h"
-#include "core_lib.h"
-#include <time.h>
-void init_shared_mem_core(aos_shm_core *shm_core) {
- clock_gettime(CLOCK_REALTIME, &shm_core->identifier);
- shm_core->queues.alloc_flag = 0;
- shm_core->msg_alloc_lock = 0;
- shm_core->queues.queue_list = NULL;
- shm_core->queues.alloc_lock = 0;
-}
+#include "aos/atom_code/ipc_lib/shared_mem.h"
+
static inline uint8_t aos_8max(uint8_t l, uint8_t r) {
- return (l > r) ? l : r;
+ return (l > r) ? l : r;
}
void *shm_malloc_aligned(size_t length, uint8_t alignment) {
- // minimum alignments from <http://software.intel.com/en-us/articles/data-alignment-when-migrating-to-64-bit-intel-architecture/>
- if (length <= 1) {
- alignment = aos_8max(alignment, 1);
- } else if (length <= 2) {
- alignment = aos_8max(alignment, 2);
- } else if (length <= 4) {
- alignment = aos_8max(alignment, 4);
- } else if (length <= 8) {
- alignment = aos_8max(alignment, 8);
- } else if (length <= 16) {
- alignment = aos_8max(alignment, 16);
- } else {
- alignment = aos_8max(alignment, (length >= 64) ? 64 : 16);
- }
+ // minimum alignments from <http://software.intel.com/en-us/articles/data-alignment-when-migrating-to-64-bit-intel-architecture/>
+ if (length <= 1) {
+ alignment = aos_8max(alignment, 1);
+ } else if (length <= 2) {
+ alignment = aos_8max(alignment, 2);
+ } else if (length <= 4) {
+ alignment = aos_8max(alignment, 4);
+ } else if (length <= 8) {
+ alignment = aos_8max(alignment, 8);
+ } else if (length <= 16) {
+ alignment = aos_8max(alignment, 16);
+ } else {
+ alignment = aos_8max(alignment, (length >= 64) ? 64 : 16);
+ }
- void *msg = NULL;
- aos_shm_core *shm_core = global_core->mem_struct;
- mutex_grab(&shm_core->msg_alloc_lock);
- shm_core->msg_alloc = (uint8_t *)shm_core->msg_alloc - length;
- const uint8_t align_extra = (uintptr_t)shm_core->msg_alloc % alignment;
- shm_core->msg_alloc = (uint8_t *)shm_core->msg_alloc - align_extra;
- msg = shm_core->msg_alloc;
- if (msg <= global_core->shared_mem) {
- fprintf(stderr, "core_lib: RAN OUT OF SHARED MEMORY!!!----------------------------------------------------------\n");
- printf("if you didn't see the stderr output just then, you should\n");
- abort();
- }
- //printf("alloc %p\n", msg);
- mutex_unlock(&shm_core->msg_alloc_lock);
- return msg;
+ void *msg = NULL;
+ aos_shm_core *shm_core = global_core->mem_struct;
+ mutex_grab(&shm_core->msg_alloc_lock);
+ shm_core->msg_alloc = (uint8_t *)shm_core->msg_alloc - length;
+ const uint8_t align_extra = (uintptr_t)shm_core->msg_alloc % alignment;
+ shm_core->msg_alloc = (uint8_t *)shm_core->msg_alloc - align_extra;
+ msg = shm_core->msg_alloc;
+ if (msg <= global_core->shared_mem) {
+ fprintf(stderr, "core_lib: RAN OUT OF SHARED MEMORY!!!----------------------------------------------------------\n");
+ printf("if you didn't see the stderr output just then, you should have\n");
+ abort();
+ }
+ //printf("alloc %p\n", msg);
+ mutex_unlock(&shm_core->msg_alloc_lock);
+ return msg;
}
diff --git a/aos/atom_code/ipc_lib/core_lib.h b/aos/atom_code/ipc_lib/core_lib.h
index f72ae4c..5674220 100644
--- a/aos/atom_code/ipc_lib/core_lib.h
+++ b/aos/atom_code/ipc_lib/core_lib.h
@@ -1,44 +1,14 @@
#ifndef _AOS_CORE_LIB_H_
#define _AOS_CORE_LIB_H_
-// defined in shared_mem.c
-#ifdef __cplusplus
-extern "C" {
-#endif // __cplusplus
-extern struct aos_core *global_core;
-#ifdef __cplusplus
-}
-#endif // __cplusplus
-
-#include "aos_sync.h"
-#include "queue.h"
#include <stdint.h>
+#include "aos/atom_code/ipc_lib/aos_sync.h"
+
#ifdef __cplusplus
extern "C" {
#endif // __cplusplus
-struct aos_queue_list_t;
-typedef struct aos_queue_hash_t {
- int alloc_flag;
- mutex alloc_lock;
- struct aos_queue_list_t *queue_list;
-} aos_queue_hash;
-
-typedef struct aos_shm_core_t {
- // clock_gettime(CLOCK_REALTIME, &identifier) gets called to identify
- // this shared memory area
- struct timespec identifier;
- // gets 0-initialized at the start (as part of shared memory) and
- // the owner sets as soon as it finishes setting stuff up
- mutex creation_condition;
- mutex msg_alloc_lock;
- void *msg_alloc;
- aos_queue_hash queues;
-} aos_shm_core;
-
-void init_shared_mem_core(aos_shm_core *shm_core);
-
void *shm_malloc_aligned(size_t length, uint8_t alignment)
__attribute__((alloc_size(1)));
static void *shm_malloc(size_t length) __attribute__((alloc_size(1)));
diff --git a/aos/atom_code/ipc_lib/ipc_lib.gyp b/aos/atom_code/ipc_lib/ipc_lib.gyp
index 4614680..f947d5e 100644
--- a/aos/atom_code/ipc_lib/ipc_lib.gyp
+++ b/aos/atom_code/ipc_lib/ipc_lib.gyp
@@ -1,41 +1,81 @@
{
'targets': [
{
- 'target_name': 'ipc_lib',
+ 'target_name': 'aos_sync',
'type': 'static_library',
'sources': [
'aos_sync.c',
- 'binheap.c',
+ ],
+ },
+ {
+ 'target_name': 'core_lib',
+ 'type': 'static_library',
+ 'sources': [
'core_lib.c',
- 'queue.c',
+ ],
+ 'dependencies': [
+ 'aos_sync',
+ 'shared_mem',
+ ],
+ 'export_dependent_settings': [
+ 'aos_sync',
+ ],
+ },
+ {
+ 'target_name': 'shared_mem',
+ 'type': 'static_library',
+ 'sources': [
'shared_mem.c',
],
'dependencies': [
+ 'aos_sync',
+ ],
+ 'export_dependent_settings': [
+ 'aos_sync',
+ ],
+ },
+ {
+ 'target_name': 'queue',
+ 'type': 'static_library',
+ 'sources': [
+ 'queue.cc',
+ ],
+ 'dependencies': [
+ '<(AOS)/common/common.gyp:condition',
+ '<(AOS)/common/common.gyp:mutex',
+ 'core_lib',
# TODO(brians): fix this once there's a nice logging interface to use
# '<(AOS)/build/aos.gyp:logging',
],
},
{
- 'target_name': 'binheap_test',
+ 'target_name': 'raw_queue_test',
'type': 'executable',
'sources': [
- 'binheap_test.cpp',
+ 'queue_test.cc',
],
'dependencies': [
'<(EXTERNALS):gtest',
- 'ipc_lib',
+ 'queue',
+ '<(AOS)/build/aos.gyp:logging',
+ 'core_lib',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/common.gyp:time',
],
},
{
- 'target_name': 'ipc_queue_test',
+ 'target_name': 'ipc_stress_test',
'type': 'executable',
'sources': [
- 'queue_test.cpp',
+ 'ipc_stress_test.cc',
],
'dependencies': [
'<(EXTERNALS):gtest',
- 'ipc_lib',
- '<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ '<(AOS)/common/common.gyp:queue_testutils',
+ '<(AOS)/common/common.gyp:mutex',
+ 'core_lib',
+ '<(AOS)/common/common.gyp:die',
],
},
],
diff --git a/aos/atom_code/ipc_lib/ipc_stress_test.cc b/aos/atom_code/ipc_lib/ipc_stress_test.cc
new file mode 100644
index 0000000..38c425f
--- /dev/null
+++ b/aos/atom_code/ipc_lib/ipc_stress_test.cc
@@ -0,0 +1,248 @@
+#include <stdio.h>
+#include <unistd.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/wait.h>
+#include <libgen.h>
+#include <assert.h>
+
+#include <vector>
+#include <string>
+
+#include "aos/common/time.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/type_traits.h"
+#include "aos/common/mutex.h"
+#include "aos/atom_code/ipc_lib/core_lib.h"
+#include "aos/common/die.h"
+
+// This runs all of the IPC-related tests in a bunch of parallel processes for a
+// while and makes sure that they don't fail. It also captures the stdout and
+// stderr output from each test run and only prints it out (not interleaved with
+// the output from any other run) if the test fails.
+//
+// It's written in C++ for performance. We need actual OS-level parallelism for
+// this to work, which means that Ruby's out because it doesn't have good
+// support for doing that. My Python implementation ended up pretty heavily disk
+// IO-bound, which is a bad way to test CPU contention.
+
+namespace aos {
+
+// Each test is represented by the name of the test binary and then any
+// arguments to pass to it.
+// Using --gtest_filter is a bad idea because it seems to result in a lot of
+// swapping which causes everything to be disk-bound (at least for me).
+static const ::std::vector< ::std::vector< ::std::string>> kTests = {
+ {"queue_test"},
+ {"condition_test"},
+ {"mutex_test"},
+ {"raw_queue_test"},
+};
+// These arguments get inserted before any per-test arguments.
+static const ::std::vector< ::std::string> kDefaultArgs = {
+ "--gtest_repeat=30",
+ "--gtest_shuffle",
+};
+
+// How many test processes to run at a time.
+static const int kTesters = 100;
+// How long to test for.
+static constexpr time::Time kTestTime = time::Time::InSeconds(30);
+
+// The structure that gets put into shared memory and then referenced by all of
+// the child processes.
+struct Shared {
+ Shared(const time::Time &stop_time)
+ : stop_time(stop_time), total_iterations(0) {}
+
+ // Synchronizes access to stdout/stderr to avoid interleaving failure
+ // messages.
+ Mutex output_mutex;
+
+ // When to stop.
+ time::Time stop_time;
+
+ // The total number of iterations. Updated by each child as it finishes.
+ int total_iterations;
+ // Sychronizes writes to total_iterations
+ Mutex total_iterations_mutex;
+
+ const char *path;
+};
+static_assert(shm_ok<Shared>::value,
+ "it's going to get shared between forked processes");
+
+// Gets called after each child forks to run a test.
+void __attribute__((noreturn)) DoRunTest(
+ Shared *shared, const ::std::vector< ::std::string> &test, int pipes[2]) {
+ if (close(pipes[0]) == -1) {
+ Die("close(%d) of read end of pipe failed with %d: %s\n",
+ pipes[0], errno, strerror(errno));
+ }
+ if (close(STDIN_FILENO) == -1) {
+ Die("close(STDIN_FILENO(=%d)) failed with %d: %s\n",
+ STDIN_FILENO, errno, strerror(errno));
+ }
+ if (dup2(pipes[1], STDOUT_FILENO) == -1) {
+ Die("dup2(%d, STDOUT_FILENO(=%d)) failed with %d: %s\n",
+ pipes[1], STDOUT_FILENO, errno, strerror(errno));
+ }
+ if (dup2(pipes[1], STDERR_FILENO) == -1) {
+ Die("dup2(%d, STDERR_FILENO(=%d)) failed with %d: %s\n",
+ pipes[1], STDERR_FILENO, errno, strerror(errno));
+ }
+
+ size_t size = test.size();
+ size_t default_size = kDefaultArgs.size();
+ const char **args = new const char *[size + default_size + 1];
+ // The actual executable to run.
+ ::std::string executable;
+ int i = 0;
+ for (const ::std::string &c : test) {
+ if (i == 0) {
+ executable = ::std::string(shared->path) + "/" + c;
+ args[0] = executable.c_str();
+ for (const ::std::string &ci : kDefaultArgs) {
+ args[++i] = ci.c_str();
+ }
+ } else {
+ args[i] = c.c_str();
+ }
+ ++i;
+ }
+ args[size] = NULL;
+ execv(executable.c_str(), const_cast<char *const *>(args));
+ Die("execv(%s, %p) failed with %d: %s\n",
+ executable.c_str(), args, errno, strerror(errno));
+}
+
+void DoRun(Shared *shared) {
+ int iterations = 0;
+ // An iterator pointing to a random one of the tests.
+ auto test = kTests.begin() + (getpid() % kTests.size());
+ int pipes[2];
+ while (time::Time::Now() < shared->stop_time) {
+ if (pipe(pipes) == -1) {
+ Die("pipe(%p) failed with %d: %s\n", &pipes, errno, strerror(errno));
+ }
+ switch (fork()) {
+ case 0: // in runner
+ DoRunTest(shared, *test, pipes);
+ case -1:
+ Die("fork() failed with %d: %s\n", errno, strerror(errno));
+ }
+
+ if (close(pipes[1]) == -1) {
+ Die("close(%d) of write end of pipe failed with %d: %s\n",
+ pipes[1], errno, strerror(errno));
+ }
+
+ ::std::string output;
+ char buffer[2048];
+ while (true) {
+ ssize_t ret = read(pipes[0], &buffer, sizeof(buffer));
+ if (ret == 0) { // EOF
+ if (close(pipes[0]) == -1) {
+ Die("close(%d) of pipe at EOF failed with %d: %s\n",
+ pipes[0], errno, strerror(errno));
+ }
+ break;
+ } else if (ret == -1) {
+ Die("read(%d, %p, %zd) failed with %d: %s\n",
+ pipes[0], &buffer, sizeof(buffer), errno, strerror(errno));
+ }
+ output += ::std::string(buffer, ret);
+ }
+
+ int status;
+ while (true) {
+ if (wait(&status) == -1) {
+ if (errno == EINTR) continue;
+ Die("wait(%p) in child failed with %d: %s\n",
+ &status, errno, strerror(errno));
+ } else {
+ break;
+ }
+ }
+ if (WIFEXITED(status)) {
+ if (WEXITSTATUS(status) != 0) {
+ MutexLocker sync(&shared->output_mutex);
+ fprintf(stderr, "Test %s exited with status %d. output:\n",
+ test->at(0).c_str(), WEXITSTATUS(status));
+ fputs(output.c_str(), stderr);
+ }
+ } else if (WIFSIGNALED(status)) {
+ MutexLocker sync(&shared->output_mutex);
+ fprintf(stderr, "Test %s terminated by signal %d: %s.\n",
+ test->at(0).c_str(),
+ WTERMSIG(status), strsignal(WTERMSIG(status)));
+ fputs(output.c_str(), stderr);
+ } else {
+ assert(WIFSTOPPED(status));
+ Die("Test %s was stopped.\n", test->at(0).c_str());
+ }
+
+ ++test;
+ if (test == kTests.end()) test = kTests.begin();
+ ++iterations;
+ }
+ {
+ MutexLocker sync(&shared->total_iterations_mutex);
+ shared->total_iterations += iterations;
+ }
+}
+
+void Run(Shared *shared) {
+ switch (fork()) {
+ case 0: // in child
+ DoRun(shared);
+ _exit(EXIT_SUCCESS);
+ case -1:
+ Die("fork() of child failed with %d: %s\n", errno, strerror(errno));
+ }
+}
+
+int Main(int argc, char **argv) {
+ assert(argc >= 1);
+
+ ::aos::common::testing::GlobalCoreInstance global_core;
+
+ Shared *shared = static_cast<Shared *>(shm_malloc(sizeof(Shared)));
+ new (shared) Shared(time::Time::Now() + kTestTime);
+
+ char *temp = strdup(argv[0]);
+ shared->path = strdup(dirname(temp));
+ free(temp);
+
+ for (int i = 0; i < kTesters; ++i) {
+ Run(shared);
+ }
+
+ bool error = false;
+ for (int i = 0; i < kTesters; ++i) {
+ int status;
+ if (wait(&status) == -1) {
+ if (errno == EINTR) {
+ --i;
+ } else {
+ Die("wait(%p) failed with %d: %s\n", &status, errno, strerror(errno));
+ }
+ }
+ if (!WIFEXITED(status) || WEXITSTATUS(status) != 0) {
+ error = true;
+ }
+ }
+
+ printf("Ran a total of %d tests.\n", shared->total_iterations);
+ if (error) {
+ printf("A child had a problem during the test.\n");
+ }
+ return error ? EXIT_FAILURE : EXIT_SUCCESS;
+}
+
+} // namespace aos
+
+int main(int argc, char **argv) {
+ return ::aos::Main(argc, argv);
+}
diff --git a/aos/atom_code/ipc_lib/mutex.cpp b/aos/atom_code/ipc_lib/mutex.cpp
index 4f908dd..47fc92a 100644
--- a/aos/atom_code/ipc_lib/mutex.cpp
+++ b/aos/atom_code/ipc_lib/mutex.cpp
@@ -26,10 +26,7 @@
}
void Mutex::Unlock() {
- if (mutex_unlock(&impl_) != 0) {
- LOG(FATAL, "mutex_unlock(%p(=%" PRIu32 ")) failed because of %d: %s\n",
- &impl_, impl_, errno, strerror(errno));
- }
+ mutex_unlock(&impl_);
}
bool Mutex::TryLock() {
diff --git a/aos/atom_code/ipc_lib/queue.c b/aos/atom_code/ipc_lib/queue.c
deleted file mode 100644
index 2e45326..0000000
--- a/aos/atom_code/ipc_lib/queue.c
+++ /dev/null
@@ -1,508 +0,0 @@
-#include "aos/atom_code/ipc_lib/queue.h"
-#include "aos/atom_code/ipc_lib/queue_internal.h"
-
-#include <stdio.h>
-#include <string.h>
-#include <errno.h>
-#include <assert.h>
-
-#include "aos/common/logging/logging.h"
-
-#define READ_DEBUG 0
-#define WRITE_DEBUG 0
-#define REF_DEBUG 0
-
-static inline aos_msg_header *get_header(void *msg) {
- return (aos_msg_header *)((uint8_t *)msg - sizeof(aos_msg_header));
-}
-static inline aos_queue *aos_core_alloc_queue() {
- return shm_malloc_aligned(sizeof(aos_queue), sizeof(int));
-}
-static inline void *aos_alloc_msg(aos_msg_pool *pool) {
- return shm_malloc(pool->msg_length);
-}
-
-// actually free the given message
-static inline int aos_free_msg(aos_msg_pool *pool, void *msg, aos_queue *queue) {
-#if REF_DEBUG
- if (pool->pool_lock == 0) {
- //LOG(WARNING, "unprotected\n");
- }
-#endif
- aos_msg_header *header = get_header(msg);
- if (pool->pool[header->index] != header) { // if something's messed up
- fprintf(stderr, "queue: something is very very wrong with queue %p."
- " pool->pool(=%p)[header->index(=%d)] != header(=%p)\n",
- queue, pool->pool, header->index, header);
- printf("queue: see stderr\n");
- abort();
- }
-#if REF_DEBUG
- printf("ref free: %p\n", msg);
-#endif
- --pool->used;
-
- if (queue->recycle != NULL) {
- void *const new_msg = aos_queue_get_msg(queue->recycle);
- if (new_msg == NULL) {
- fprintf(stderr, "queue: couldn't get a message"
- " for recycle queue %p\n", queue->recycle);
- } else {
- // Take a message from recycle_queue and switch its
- // header with the one being freed, which effectively
- // switches which queue each message belongs to.
- aos_msg_header *const new_header = get_header(new_msg);
- // also switch the messages between the pools
- pool->pool[header->index] = new_header;
- if (mutex_lock(&queue->recycle->pool.pool_lock)) {
- return -1;
- }
- queue->recycle->pool.pool[new_header->index] = header;
- // swap the information in both headers
- header_swap(header, new_header);
- // don't unlock the other pool until all of its messages are valid
- mutex_unlock(&queue->recycle->pool.pool_lock);
- // use the header for new_msg which is now for this pool
- header = new_header;
- if (aos_queue_write_msg_free(queue->recycle,
- (void *)msg, OVERRIDE) != 0) {
- printf("queue: warning aos_queue_write_msg("
- "%p(=queue(=%p)->recycle), %p, OVERRIDE)"
- " failed\n",
- queue->recycle, queue, msg);
- }
- msg = new_msg;
- }
- }
-
- // where the one we're freeing was
- int index = header->index;
- header->index = -1;
- if (index != pool->used) { // if we're not freeing the one on the end
- // put the last one where the one we're freeing was
- header = pool->pool[index] = pool->pool[pool->used];
- // put the one we're freeing at the end
- pool->pool[pool->used] = get_header(msg);
- // update the former last one's index
- header->index = index;
- }
- return 0;
-}
-// TODO(brians) maybe do this with atomic integer instructions so it doesn't have to lock/unlock pool_lock
-static inline int msg_ref_dec(void *msg, aos_msg_pool *pool, aos_queue *queue) {
- if (msg == NULL) {
- return 0;
- }
-
- int rv = 0;
- if (mutex_lock(&pool->pool_lock)) {
- return -1;
- }
- aos_msg_header *const header = get_header(msg);
- header->ref_count --;
- assert(header->ref_count >= 0);
-#if REF_DEBUG
- printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
-#endif
- if (header->ref_count == 0) {
- rv = aos_free_msg(pool, msg, queue);
- }
- mutex_unlock(&pool->pool_lock);
- return rv;
-}
-
-static inline int sigcmp(const aos_type_sig *sig1, const aos_type_sig *sig2) {
- if (sig1->length != sig2->length) {
- //LOG(DEBUG, "length mismatch 1=%d 2=%d\n", sig1->length, sig2->length);
- return 0;
- }
- if (sig1->queue_length != sig2->queue_length) {
- //LOG(DEBUG, "queue_length mismatch 1=%d 2=%d\n", sig1->queue_length, sig2->queue_length);
- return 0;
- }
- if (sig1->hash != sig2->hash) {
- //LOG(DEBUG, "hash mismatch 1=%d 2=%d\n", sig1->hash, sig2->hash);
- return 0;
- }
- //LOG(DEBUG, "signature match\n");
- return 1;
-}
-static inline aos_queue *aos_create_queue(const aos_type_sig *sig) {
- aos_queue *const queue = aos_core_alloc_queue();
- aos_msg_pool *const pool = &queue->pool;
- pool->mem_length = sig->queue_length + EXTRA_MESSAGES;
- pool->length = 0;
- pool->used = 0;
- pool->msg_length = sig->length + sizeof(aos_msg_header);
- pool->pool = shm_malloc(sizeof(void *) * pool->mem_length);
- aos_ring_buf *const buf = &queue->buf;
- buf->length = sig->queue_length + 1;
- if (buf->length < 2) { // TODO(brians) when could this happen?
- buf->length = 2;
- }
- buf->data = shm_malloc(buf->length * sizeof(void *));
- buf->start = 0;
- buf->end = 0;
- buf->msgs = 0;
- buf->writable = 1;
- buf->readable = 0;
- buf->buff_lock = 0;
- pool->pool_lock = 0;
- queue->recycle = NULL;
- return queue;
-}
-aos_queue *aos_fetch_queue(const char *name, const aos_type_sig *sig) {
- //LOG(DEBUG, "Fetching the stupid queue: %s\n", name);
- mutex_grab(&global_core->mem_struct->queues.alloc_lock);
- aos_queue_list *list = global_core->mem_struct->queues.queue_list;
- aos_queue_list *last = NULL;
- while (list != NULL) {
- // if we found a matching queue
- if (strcmp(list->name, name) == 0 && sigcmp(&list->sig, sig)) {
- mutex_unlock(&global_core->mem_struct->queues.alloc_lock);
- return list->queue;
- } else {
- //LOG(DEBUG, "rejected queue %s strcmp=%d target=%s\n", (*list)->name, strcmp((*list)->name, name), name);
- }
- last = list;
- list = list->next;
- }
- list = shm_malloc(sizeof(aos_queue_list));
- if (last == NULL) {
- global_core->mem_struct->queues.queue_list = list;
- } else {
- last->next = list;
- }
- list->sig = *sig;
- const size_t name_size = strlen(name) + 1;
- list->name = shm_malloc(name_size);
- memcpy(list->name, name, name_size);
- //LOG(INFO, "creating queue{name=%s, sig.length=%zd, sig.hash=%d, sig.queue_length=%d}\n", name, sig->length, sig->hash, sig->queue_length);
- list->queue = aos_create_queue(sig);
- //LOG(DEBUG, "Made the stupid queue: %s happy?\n", name);
- list->next = NULL;
- mutex_unlock(&global_core->mem_struct->queues.alloc_lock);
- return list->queue;
-}
-aos_queue *aos_fetch_queue_recycle(const char *name, const aos_type_sig *sig,
- const aos_type_sig *recycle_sig, aos_queue **recycle) {
- if (sig->length != recycle_sig->length || sig->hash == recycle_sig->hash) {
- *recycle = NULL;
- return NULL;
- }
- aos_queue *const r = aos_fetch_queue(name, sig);
- r->recycle = aos_fetch_queue(name, recycle_sig);
- if (r == r->recycle) {
- fprintf(stderr, "queue: r->recycle(=%p) == r(=%p)\n", r->recycle, r);
- printf("see stderr\n");
- abort();
- }
- *recycle = r->recycle;
- return r;
-}
-
-int aos_queue_write_msg(aos_queue *queue, void *msg, int opts) {
-#if WRITE_DEBUG
- printf("queue: write_msg(%p, %p, %d)\n", queue, msg, opts);
-#endif
- int rv = 0;
- if (msg == NULL || msg < (void *)global_core->mem_struct ||
- msg > (void *)((intptr_t)global_core->mem_struct + global_core->size)) {
- fprintf(stderr, "queue: attempt to write bad message %p to %p. aborting\n",
- msg, queue);
- printf("see stderr\n");
- abort();
- }
- aos_ring_buf *const buf = &queue->buf;
- if (mutex_lock(&buf->buff_lock)) {
-#if WRITE_DEBUG
- printf("queue: locking buff_lock of %p failed\n", buf);
-#endif
- return -1;
- }
- int new_end = (buf->end + 1) % buf->length;
- while (new_end == buf->start) {
- if (opts & NON_BLOCK) {
-#if WRITE_DEBUG
- printf("queue: not blocking on %p. returning -1\n", queue);
-#endif
- mutex_unlock(&buf->buff_lock);
- return -1;
- } else if (opts & OVERRIDE) {
-#if WRITE_DEBUG
- printf("queue: overriding on %p\n", queue);
-#endif
- // avoid leaking the message that we're going to overwrite
- msg_ref_dec(buf->data[buf->start], &queue->pool, queue);
- buf->start = (buf->start + 1) % buf->length;
- } else { // BLOCK
- mutex_unlock(&buf->buff_lock);
-#if WRITE_DEBUG
- printf("queue: going to wait for writable(=%p) of %p\n",
- &buf->writable, queue);
-#endif
- if (condition_wait(&buf->writable)) {
-#if WRITE_DEBUG
- printf("queue: waiting for writable(=%p) of %p failed\n",
- &buf->writable, queue);
-#endif
- return -1;
- }
-#if WRITE_DEBUG
- printf("queue: going to re-lock buff_lock of %p to write\n", queue);
-#endif
- if (mutex_lock(&buf->buff_lock)) {
-#if WRITE_DEBUG
- printf("queue: error locking buff_lock of %p\n", queue);
-#endif
- return -1;
- }
- }
- new_end = (buf->end + 1) % buf->length;
- }
- buf->data[buf->end] = msg;
- ++buf->msgs;
- buf->end = new_end;
- mutex_unlock(&buf->buff_lock);
-#if WRITE_DEBUG
- printf("queue: setting readable(=%p) of %p\n", &buf->readable, queue);
-#endif
- condition_set(&buf->readable);
- if (((buf->end + 1) % buf->length) == buf->start) { // if it's now full
- condition_unset(&buf->writable);
- }
-#if WRITE_DEBUG
- printf("queue: write returning %d on queue %p\n", rv, queue);
-#endif
- return rv;
-}
-
-int aos_queue_free_msg(aos_queue *queue, const void *msg) {
- // TODO(brians) get rid of this
- void *msg_temp;
- memcpy(&msg_temp, &msg, sizeof(msg_temp));
- return msg_ref_dec(msg_temp, &queue->pool, queue);
-}
-// Deals with setting/unsetting readable and writable.
-// Should be called after buff_lock has been unlocked.
-// read is whether or not this read call read one off the queue
-static inline void aos_read_msg_common_end(aos_ring_buf *const buf, int read) {
- if (read) {
- condition_set(&buf->writable);
- if (buf->start == buf->end) {
- condition_unset(&buf->readable);
- }
- }
-}
-// Returns with buff_lock locked and a readable message in buf.
-// Returns -1 for error (if it returns -1, buff_lock will be unlocked).
-static inline int aos_queue_read_msg_common(int opts, aos_ring_buf *const buf,
- aos_queue *const queue, int *index) {
-#if !READ_DEBUG
- (void)queue;
-#endif
- if (mutex_lock(&buf->buff_lock)) {
-#if READ_DEBUG
- printf("queue: couldn't lock buff_lock of %p\n", queue);
-#endif
- return -1;
- }
- while (buf->start == buf->end || ((index != NULL) && buf->msgs <= *index)) {
- mutex_unlock(&buf->buff_lock);
- if (opts & NON_BLOCK) {
-#if READ_DEBUG
- printf("queue: not going to block waiting on %p\n", queue);
-#endif
- return -1;
- } else { // BLOCK
-#if READ_DEBUG
- printf("queue: going to wait for readable(=%p) of %p\n",
- &buf->readable, queue);
-#endif
- // wait for a message to become readable
- if ((index == NULL) ? condition_wait(&buf->readable) :
- condition_wait_force(&buf->readable)) {
-#if READ_DEBUG
- printf("queue: waiting for readable(=%p) of %p failed\n",
- &buf->readable, queue);
-#endif
- return -1;
- }
- }
-#if READ_DEBUG
- printf("queue: going to re-lock buff_lock of %p to read\n", queue);
-#endif
- if (mutex_lock(&buf->buff_lock)) {
-#if READ_DEBUG
- printf("couldn't re-lock buff_lock of %p\n", queue);
-#endif
- return -1;
- }
- }
-#if READ_DEBUG
- printf("queue: read start=%d end=%d from %p\n", buf->start, buf->end, queue);
-#endif
- return 0;
-}
-// handles reading with PEEK
-static inline void *read_msg_peek(aos_ring_buf *const buf, int opts, int start) {
- void *ret;
- if (opts & FROM_END) {
- int pos = buf->end - 1;
- if (pos < 0) { // if it needs to wrap
- pos = buf->length - 1;
- }
-#if READ_DEBUG
- printf("queue: reading from line %d: %d\n", __LINE__, pos);
-#endif
- ret = buf->data[pos];
- } else {
-#if READ_DEBUG
- printf("queue: reading from line %d: %d\n", __LINE__, start);
-#endif
- ret = buf->data[start];
- }
- aos_msg_header *const header = get_header(ret);
- header->ref_count ++;
-#if REF_DEBUG
- printf("ref inc count: %p\n", ret);
-#endif
- return ret;
-}
-const void *aos_queue_read_msg(aos_queue *queue, int opts) {
-#if READ_DEBUG
- printf("queue: read_msg(%p, %d)\n", queue, opts);
-#endif
- void *msg = NULL;
- aos_ring_buf *const buf = &queue->buf;
- if (aos_queue_read_msg_common(opts, buf, queue, NULL) == -1) {
-#if READ_DEBUG
- printf("queue: common returned -1 for %p\n", queue);
-#endif
- return NULL;
- }
- if (opts & PEEK) {
- msg = read_msg_peek(buf, opts, buf->start);
- } else {
- if (opts & FROM_END) {
- while (1) {
-#if READ_DEBUG
- printf("queue: start of c2 of %p\n", queue);
-#endif
- // This loop pulls each message out of the buffer.
- const int pos = buf->start;
- buf->start = (buf->start + 1) % buf->length;
- // if this is the last one
- if (buf->start == buf->end) {
-#if READ_DEBUG
- printf("queue: reading from c2: %d\n", pos);
-#endif
- msg = buf->data[pos];
- break;
- }
- // it's not going to be in the queue any more
- msg_ref_dec(buf->data[pos], &queue->pool, queue);
- }
- } else {
-#if READ_DEBUG
- printf("queue: reading from d2: %d\n", buf->start);
-#endif
- msg = buf->data[buf->start];
- buf->start = (buf->start + 1) % buf->length;
- }
- }
- mutex_unlock(&buf->buff_lock);
- aos_read_msg_common_end(buf, !(opts & PEEK));
-#if READ_DEBUG
- printf("queue: read returning %p\n", msg);
-#endif
- return msg;
-}
-const void *aos_queue_read_msg_index(aos_queue *queue, int opts, int *index) {
-#if READ_DEBUG
- printf("queue: read_msg_index(%p, %d, %p(*=%d))\n", queue, opts, index, *index);
-#endif
- void *msg = NULL;
- aos_ring_buf *const buf = &queue->buf;
- if (aos_queue_read_msg_common(opts, buf, queue, index) == -1) {
-#if READ_DEBUG
- printf("queue: common returned -1\n");
-#endif
- return NULL;
- }
- // TODO(parker): Handle integer wrap on the index.
- const int offset = buf->msgs - *index;
- int my_start = buf->end - offset;
- if (offset >= buf->length) { // if we're behind the available messages
- // catch index up to the last available message
- *index += buf->start - my_start;
- // and that's the one we're going to read
- my_start = buf->start;
- }
- if (my_start < 0) { // if we want to read off the end of the buffer
- // unwrap where we're going to read from
- my_start += buf->length;
- }
- if (opts & PEEK) {
- msg = read_msg_peek(buf, opts, my_start);
- } else {
- if (opts & FROM_END) {
-#if READ_DEBUG
- printf("queue: start of c1 of %p\n", queue);
-#endif
- int pos = buf->end - 1;
- if (pos < 0) { // if it wrapped
- pos = buf->length - 1; // unwrap it
- }
-#if READ_DEBUG
- printf("queue: reading from c1: %d\n", pos);
-#endif
- msg = buf->data[pos];
- *index = buf->msgs;
- } else {
-#if READ_DEBUG
- printf("queue: reading from d1: %d\n", my_start);
-#endif
- msg = buf->data[my_start];
- ++(*index);
- }
- aos_msg_header *const header = get_header(msg);
- ++header->ref_count;
-#if REF_DEBUG
- printf("ref_inc_count: %p\n", msg);
-#endif
- }
- mutex_unlock(&buf->buff_lock);
- // this function never consumes one off the queue
- aos_read_msg_common_end(buf, 0);
- return msg;
-}
-static inline void *aos_pool_get_msg(aos_msg_pool *pool) {
- if (mutex_lock(&pool->pool_lock)) {
- return NULL;
- }
- void *msg;
- if (pool->length - pool->used > 0) {
- msg = pool->pool[pool->used];
- } else {
- if (pool->length >= pool->mem_length) {
- LOG(FATAL, "overused pool %p\n", pool);
- }
- msg = pool->pool[pool->length] = aos_alloc_msg(pool);
- ++pool->length;
- }
- aos_msg_header *const header = msg;
- msg = (uint8_t *)msg + sizeof(aos_msg_header);
- header->ref_count = 1;
-#if REF_DEBUG
- printf("ref alloc: %p\n", msg);
-#endif
- header->index = pool->used;
- ++pool->used;
- mutex_unlock(&pool->pool_lock);
- return msg;
-}
-void *aos_queue_get_msg(aos_queue *queue) {
- return aos_pool_get_msg(&queue->pool);
-}
-
diff --git a/aos/atom_code/ipc_lib/queue.cc b/aos/atom_code/ipc_lib/queue.cc
new file mode 100644
index 0000000..7ab7b6c
--- /dev/null
+++ b/aos/atom_code/ipc_lib/queue.cc
@@ -0,0 +1,491 @@
+#include "aos/atom_code/ipc_lib/queue.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <errno.h>
+#include <assert.h>
+
+#include <memory>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/type_traits.h"
+#include "aos/atom_code/ipc_lib/core_lib.h"
+
+namespace aos {
+namespace {
+
+static_assert(shm_ok<RawQueue>::value,
+ "RawQueue instances go into shared memory");
+
+const bool kReadDebug = false;
+const bool kWriteDebug = false;
+const bool kRefDebug = false;
+const bool kFetchDebug = false;
+
+// The number of extra messages the pool associated with each queue will be able
+// to hold (for readers who are slow about freeing them or who leak one when
+// they get killed).
+const int kExtraMessages = 20;
+
+} // namespace
+
+const int RawQueue::kPeek;
+const int RawQueue::kFromEnd;
+const int RawQueue::kNonBlock;
+const int RawQueue::kBlock;
+const int RawQueue::kOverride;
+
+struct RawQueue::MessageHeader {
+ int ref_count;
+ int index; // in pool_
+ static MessageHeader *Get(const void *msg) {
+ return reinterpret_cast<MessageHeader *>(
+ static_cast<uint8_t *>(const_cast<void *>(msg)) -
+ sizeof(MessageHeader));
+ }
+ void Swap(MessageHeader *other) {
+ MessageHeader temp;
+ memcpy(&temp, other, sizeof(temp));
+ memcpy(other, this, sizeof(*other));
+ memcpy(this, &temp, sizeof(*this));
+ }
+};
+static_assert(shm_ok<RawQueue::MessageHeader>::value, "the whole point"
+ " is to stick it in shared memory");
+
+struct RawQueue::ReadData {
+ bool writable_start;
+};
+
+// TODO(brians) maybe do this with atomic integer instructions so it doesn't
+// have to lock/unlock pool_lock_
+void RawQueue::DecrementMessageReferenceCount(const void *msg) {
+ MutexLocker locker(&pool_lock_);
+ MessageHeader *header = MessageHeader::Get(msg);
+ --header->ref_count;
+ assert(header->ref_count >= 0);
+ if (kRefDebug) {
+ printf("ref_dec_count: %p count=%d\n", msg, header->ref_count);
+ }
+ if (header->ref_count == 0) {
+ DoFreeMessage(msg);
+ }
+}
+
+RawQueue::RawQueue(const char *name, size_t length, int hash, int queue_length)
+ : readable_(&data_lock_), writable_(&data_lock_) {
+ const size_t name_size = strlen(name) + 1;
+ char *temp = static_cast<char *>(shm_malloc(name_size));
+ memcpy(temp, name, name_size);
+ name_ = temp;
+ length_ = length;
+ hash_ = hash;
+ queue_length_ = queue_length;
+
+ next_ = NULL;
+ recycle_ = NULL;
+
+ if (kFetchDebug) {
+ printf("initializing name=%s, length=%zd, hash=%d, queue_length=%d\n",
+ name, length, hash, queue_length);
+ }
+
+ data_length_ = queue_length + 1;
+ if (data_length_ < 2) { // TODO(brians) when could this happen?
+ data_length_ = 2;
+ }
+ data_ = static_cast<void **>(shm_malloc(sizeof(void *) * data_length_));
+ data_start_ = 0;
+ data_end_ = 0;
+ messages_ = 0;
+
+ mem_length_ = queue_length + kExtraMessages;
+ pool_length_ = 0;
+ messages_used_ = 0;
+ msg_length_ = length + sizeof(MessageHeader);
+ pool_ = static_cast<MessageHeader **>(
+ shm_malloc(sizeof(MessageHeader *) * mem_length_));
+
+ if (kFetchDebug) {
+ printf("made queue %s\n", name);
+ }
+}
+RawQueue *RawQueue::Fetch(const char *name, size_t length, int hash,
+ int queue_length) {
+ if (kFetchDebug) {
+ printf("fetching queue %s\n", name);
+ }
+ if (mutex_lock(&global_core->mem_struct->queues.alloc_lock) != 0) {
+ return NULL;
+ }
+ RawQueue *current = static_cast<RawQueue *>(
+ global_core->mem_struct->queues.queue_list);
+ if (current != NULL) {
+ while (true) {
+ // If we found a matching queue.
+ if (strcmp(current->name_, name) == 0 && current->length_ == length &&
+ current->hash_ == hash && current->queue_length_ == queue_length) {
+ mutex_unlock(&global_core->mem_struct->queues.alloc_lock);
+ return current;
+ } else {
+ if (kFetchDebug) {
+ printf("rejected queue %s strcmp=%d target=%s\n", current->name_,
+ strcmp(current->name_, name), name);
+ }
+ }
+ // If this is the last one.
+ if (current->next_ == NULL) break;
+ current = current->next_;
+ }
+ }
+
+ RawQueue *r = new (shm_malloc(sizeof(RawQueue)))
+ RawQueue(name, length, hash, queue_length);
+ if (current == NULL) { // if we don't already have one
+ global_core->mem_struct->queues.queue_list = r;
+ } else {
+ current->next_ = r;
+ }
+
+ mutex_unlock(&global_core->mem_struct->queues.alloc_lock);
+ return r;
+}
+RawQueue *RawQueue::Fetch(const char *name, size_t length, int hash,
+ int queue_length,
+ int recycle_hash, int recycle_length, RawQueue **recycle) {
+ RawQueue *r = Fetch(name, length, hash, queue_length);
+ r->recycle_ = Fetch(name, length, recycle_hash, recycle_length);
+ if (r == r->recycle_) {
+ fprintf(stderr, "queue: r->recycle_(=%p) == r(=%p)\n", r->recycle_, r);
+ printf("see stderr\n");
+ r->recycle_ = NULL;
+ abort();
+ }
+ *recycle = r->recycle_;
+ return r;
+}
+
+void RawQueue::DoFreeMessage(const void *msg) {
+ MessageHeader *header = MessageHeader::Get(msg);
+ if (pool_[header->index] != header) { // if something's messed up
+ fprintf(stderr, "queue: something is very very wrong with queue %p."
+ " pool_(=%p)[header->index(=%d)] != header(=%p)\n",
+ this, pool_, header->index, header);
+ printf("queue: see stderr\n");
+ abort();
+ }
+ if (kRefDebug) {
+ printf("ref free: %p\n", msg);
+ }
+ --messages_used_;
+
+ if (recycle_ != NULL) {
+ void *const new_msg = recycle_->GetMessage();
+ if (new_msg == NULL) {
+ fprintf(stderr, "queue: couldn't get a message"
+ " for recycle queue %p\n", recycle_);
+ } else {
+ // Take a message from recycle_ and switch its
+ // header with the one being freed, which effectively
+ // switches which queue each message belongs to.
+ MessageHeader *const new_header = MessageHeader::Get(new_msg);
+ // Also switch the messages between the pools.
+ pool_[header->index] = new_header;
+ {
+ MutexLocker locker(&recycle_->pool_lock_);
+ recycle_->pool_[new_header->index] = header;
+ // Swap the information in both headers.
+ header->Swap(new_header);
+ // Don't unlock the other pool until all of its messages are valid.
+ }
+ // use the header for new_msg which is now for this pool
+ header = new_header;
+ if (!recycle_->WriteMessage(const_cast<void *>(msg), kOverride)) {
+ fprintf(stderr, "queue: %p->WriteMessage(%p, kOverride) failed."
+ " aborting\n", recycle_, msg);
+ printf("see stderr\n");
+ abort();
+ }
+ msg = new_msg;
+ }
+ }
+
+ // Where the one we're freeing was.
+ int index = header->index;
+ header->index = -1;
+ if (index != messages_used_) { // if we're not freeing the one on the end
+ // Put the last one where the one we're freeing was.
+ header = pool_[index] = pool_[messages_used_];
+ // Put the one we're freeing at the end.
+ pool_[messages_used_] = MessageHeader::Get(msg);
+ // Update the former last one's index.
+ header->index = index;
+ }
+}
+
+bool RawQueue::WriteMessage(void *msg, int options) {
+ if (kWriteDebug) {
+ printf("queue: %p->WriteMessage(%p, %x)\n", this, msg, options);
+ }
+ if (msg == NULL || msg < reinterpret_cast<void *>(global_core->mem_struct) ||
+ msg > static_cast<void *>((
+ reinterpret_cast<char *>(global_core->mem_struct) +
+ global_core->size))) {
+ fprintf(stderr, "queue: attempt to write bad message %p to %p. aborting\n",
+ msg, this);
+ printf("see stderr\n");
+ abort();
+ }
+ {
+ MutexLocker locker(&data_lock_);
+ bool writable_waited = false;
+
+ int new_end;
+ while (true) {
+ new_end = (data_end_ + 1) % data_length_;
+ // If there is room in the queue right now.
+ if (new_end != data_start_) break;
+ if (options & kNonBlock) {
+ if (kWriteDebug) {
+ printf("queue: not blocking on %p. returning false\n", this);
+ }
+ return false;
+ } else if (options & kOverride) {
+ if (kWriteDebug) {
+ printf("queue: overriding on %p\n", this);
+ }
+ // Avoid leaking the message that we're going to overwrite.
+ DecrementMessageReferenceCount(data_[data_start_]);
+ data_start_ = (data_start_ + 1) % data_length_;
+ } else { // kBlock
+ if (kWriteDebug) {
+ printf("queue: going to wait for writable_ of %p\n", this);
+ }
+ writable_.Wait();
+ writable_waited = true;
+ }
+ }
+ data_[data_end_] = msg;
+ ++messages_;
+ data_end_ = new_end;
+
+ if (kWriteDebug) {
+ printf("queue: broadcasting to readable_ of %p\n", this);
+ }
+ readable_.Broadcast();
+
+ // If we got a signal on writable_ here and it's still writable, then we
+ // need to signal the next person in line (if any).
+ if (writable_waited && is_writable()) {
+ if (kWriteDebug) {
+ printf("queue: resignalling writable_ of %p\n", this);
+ }
+ writable_.Signal();
+ }
+ }
+ if (kWriteDebug) {
+ printf("queue: write returning true on queue %p\n", this);
+ }
+ return true;
+}
+
+void RawQueue::ReadCommonEnd(ReadData *read_data) {
+ if (is_writable()) {
+ if (kReadDebug) {
+ printf("queue: %ssignalling writable_ of %p\n",
+ read_data->writable_start ? "not " : "", this);
+ }
+ if (!read_data->writable_start) writable_.Signal();
+ }
+}
+bool RawQueue::ReadCommonStart(int options, int *index, ReadData *read_data) {
+ read_data->writable_start = is_writable();
+ while (data_start_ == data_end_ || ((index != NULL) && messages_ <= *index)) {
+ if (options & kNonBlock) {
+ if (kReadDebug) {
+ printf("queue: not going to block waiting on %p\n", this);
+ }
+ return false;
+ } else { // kBlock
+ if (kReadDebug) {
+ printf("queue: going to wait for readable_ of %p\n", this);
+ }
+ // Wait for a message to become readable.
+ readable_.Wait();
+ if (kReadDebug) {
+ printf("queue: done waiting for readable_ of %p\n", this);
+ }
+ }
+ }
+ if (kReadDebug) {
+ printf("queue: %p->read start=%d end=%d\n", this, data_start_, data_end_);
+ }
+ return true;
+}
+void *RawQueue::ReadPeek(int options, int start) {
+ void *ret;
+ if (options & kFromEnd) {
+ int pos = data_end_ - 1;
+ if (pos < 0) { // if it needs to wrap
+ pos = data_length_ - 1;
+ }
+ if (kReadDebug) {
+ printf("queue: %p reading from line %d: %d\n", this, __LINE__, pos);
+ }
+ ret = data_[pos];
+ } else {
+ if (kReadDebug) {
+ printf("queue: %p reading from line %d: %d\n", this, __LINE__, start);
+ }
+ ret = data_[start];
+ }
+ MessageHeader *const header = MessageHeader::Get(ret);
+ ++header->ref_count;
+ if (kRefDebug) {
+ printf("ref inc count: %p\n", ret);
+ }
+ return ret;
+}
+const void *RawQueue::ReadMessage(int options) {
+ if (kReadDebug) {
+ printf("queue: %p->ReadMessage(%x)\n", this, options);
+ }
+ void *msg = NULL;
+
+ MutexLocker locker(&data_lock_);
+
+ ReadData read_data;
+ if (!ReadCommonStart(options, NULL, &read_data)) {
+ if (kReadDebug) {
+ printf("queue: %p common returned false\n", this);
+ }
+ return NULL;
+ }
+
+ if (options & kPeek) {
+ msg = ReadPeek(options, data_start_);
+ } else {
+ if (options & kFromEnd) {
+ while (true) {
+ if (kReadDebug) {
+ printf("queue: %p start of c2\n", this);
+ }
+ // This loop pulls each message out of the buffer.
+ const int pos = data_start_;
+ data_start_ = (data_start_ + 1) % data_length_;
+ // If this is the last one.
+ if (data_start_ == data_end_) {
+ if (kReadDebug) {
+ printf("queue: %p reading from c2: %d\n", this, pos);
+ }
+ msg = data_[pos];
+ break;
+ }
+ // This message is not going to be in the queue any more.
+ DecrementMessageReferenceCount(data_[pos]);
+ }
+ } else {
+ if (kReadDebug) {
+ printf("queue: %p reading from d2: %d\n", this, data_start_);
+ }
+ msg = data_[data_start_];
+ // TODO(brians): Doesn't this need to increment the ref count?
+ data_start_ = (data_start_ + 1) % data_length_;
+ }
+ }
+ ReadCommonEnd(&read_data);
+ if (kReadDebug) {
+ printf("queue: %p read returning %p\n", this, msg);
+ }
+ return msg;
+}
+const void *RawQueue::ReadMessageIndex(int options, int *index) {
+ if (kReadDebug) {
+ printf("queue: %p->ReadMessageIndex(%x, %p(*=%d))\n",
+ this, options, index, *index);
+ }
+ void *msg = NULL;
+
+ MutexLocker locker(&data_lock_);
+
+ ReadData read_data;
+ if (!ReadCommonStart(options, index, &read_data)) {
+ if (kReadDebug) {
+ printf("queue: %p common returned false\n", this);
+ }
+ return NULL;
+ }
+
+ // TODO(parker): Handle integer wrap on the index.
+
+ // How many unread messages we have.
+ const int offset = messages_ - *index;
+ // Where we're going to start reading.
+ int my_start = data_end_ - offset;
+ if (my_start < 0) { // If we want to read off the end of the buffer.
+ // Unwrap it.
+ my_start += data_length_;
+ }
+ if (offset >= data_length_) { // If we're behind the available messages.
+ // Catch index up to the last available message.
+ *index += data_start_ - my_start;
+ // And that's the one we're going to read.
+ my_start = data_start_;
+ }
+ if (options & kPeek) {
+ msg = ReadPeek(options, my_start);
+ } else {
+ if (options & kFromEnd) {
+ if (kReadDebug) {
+ printf("queue: %p start of c1\n", this);
+ }
+ int pos = data_end_ - 1;
+ if (pos < 0) { // If it wrapped.
+ pos = data_length_ - 1; // Unwrap it.
+ }
+ if (kReadDebug) {
+ printf("queue: %p reading from c1: %d\n", this, pos);
+ }
+ msg = data_[pos];
+ *index = messages_;
+ } else {
+ if (kReadDebug) {
+ printf("queue: %p reading from d1: %d\n", this, my_start);
+ }
+ msg = data_[my_start];
+ ++(*index);
+ }
+ MessageHeader *const header = MessageHeader::Get(msg);
+ ++header->ref_count;
+ if (kRefDebug) {
+ printf("ref_inc_count: %p\n", msg);
+ }
+ }
+ ReadCommonEnd(&read_data);
+ return msg;
+}
+
+void *RawQueue::GetMessage() {
+ MutexLocker locker(&pool_lock_);
+ MessageHeader *header;
+ if (pool_length_ - messages_used_ > 0) {
+ header = pool_[messages_used_];
+ } else {
+ if (pool_length_ >= mem_length_) {
+ LOG(FATAL, "overused pool of queue %p\n", this);
+ }
+ header = pool_[pool_length_] =
+ static_cast<MessageHeader *>(shm_malloc(msg_length_));
+ ++pool_length_;
+ }
+ void *msg = reinterpret_cast<uint8_t *>(header) + sizeof(MessageHeader);
+ header->ref_count = 1;
+ if (kRefDebug) {
+ printf("%p ref alloc: %p\n", this, msg);
+ }
+ header->index = messages_used_;
+ ++messages_used_;
+ return msg;
+}
+
+} // namespace aos
diff --git a/aos/atom_code/ipc_lib/queue.h b/aos/atom_code/ipc_lib/queue.h
index 4c279e1..5158558 100644
--- a/aos/atom_code/ipc_lib/queue.h
+++ b/aos/atom_code/ipc_lib/queue.h
@@ -1,134 +1,171 @@
-#ifndef AOS_IPC_LIB_QUEUE_H_
-#define AOS_IPC_LIB_QUEUE_H_
+#ifndef AOS_ATOM_CODE_IPC_LIB_QUEUE_H_
+#define AOS_ATOM_CODE_IPC_LIB_QUEUE_H_
-#include "shared_mem.h"
-#include "aos_sync.h"
+#include "aos/atom_code/ipc_lib/shared_mem.h"
+#include "aos/common/mutex.h"
+#include "aos/common/condition.h"
// TODO(brians) add valgrind client requests to the queue and shared_mem_malloc
// code to make checking for leaks work better
// <http://www.valgrind.org/docs/manual/mc-manual.html#mc-manual.mempools>
// describes how
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-// Queues are the primary way to use shared memory. Basic use consists of
-// initializing an aos_type_sig and then calling aos_fetch_queue on it.
-// This aos_queue* can then be used to get a message and write it or to read a
-// message.
-// Queues (as the name suggests) are a FIFO stack of messages. Each combination
-// of name and aos_type_sig will result in a different queue, which means that
-// if you only recompile some code that uses differently sized messages, it will
-// simply use a different queue than the old code.
-//
// Any pointers returned from these functions can be safely passed to other
// processes because they are all shared memory pointers.
// IMPORTANT: Any message pointer must be passed back in some way
-// (aos_queue_free_msg and aos_queue_write_msg are common ones) or the
+// (FreeMessage and WriteMessage are common ones) or the
// application will leak shared memory.
-// NOTE: Taking a message from read_msg and then passing it to write_msg might
-// work, but it is not guaranteed to.
+// NOTE: Taking a message from ReadMessage and then passing it to WriteMessage
+// might work, but it is not guaranteed to.
-typedef struct aos_type_sig_t {
- size_t length; // sizeof(message)
- int hash; // can differentiate multiple otherwise identical queues
- int queue_length; // how many messages the queue can hold
-} aos_type_sig;
+namespace aos {
-// Structures that are opaque to users (defined in queue_internal.h).
-typedef struct aos_queue_list_t aos_queue_list;
-typedef struct aos_queue_t aos_queue;
+// Queues are the primary way to use shared memory. Basic use consists of
+// calling Queue::Fetch and then reading and/or writing messages.
+// Queues (as the name suggests) are a FIFO stack of messages. Each combination
+// of name and type signature will result in a different queue, which means
+// that if you only recompile some code that uses differently sized messages,
+// it will simply use a different queue than the old code.
+class RawQueue {
+ public:
+ // Retrieves (and creates if necessary) a queue. Each combination of name and
+ // signature refers to a completely independent queue.
+ // length is how large each message will be
+ // hash can differentiate multiple otherwise identical queues
+ // queue_length is how many messages the queue will be able to hold
+ static RawQueue *Fetch(const char *name, size_t length, int hash,
+ int queue_length);
+ // Same as above, except sets up the returned queue so that it will put
+ // messages on *recycle when they are freed (after they have been released by
+ // all other readers/writers and are not in the queue).
+ // recycle_queue_length determines how many freed messages will be kept.
+ // Other code can retrieve the 2 queues separately (the recycle queue will
+ // have the same length and hash as the main one). However, any frees made
+ // using a queue with only (name,length,hash,queue_length) before the
+ // recycle queue has been associated with it will not go on to the recycle
+ // queue.
+ // NOTE: calling this function with the same (name,length,hash,queue_length)
+ // but multiple recycle_queue_lengths will result in each freed message being
+ // put onto an undefined one of the recycle queues.
+ static RawQueue *Fetch(const char *name, size_t length, int hash,
+ int queue_length,
+ int recycle_hash, int recycle_queue_length,
+ RawQueue **recycle);
-// Retrieves (and creates if necessary) a queue. Each combination of name and
-// signature refers to a completely independent queue.
-aos_queue *aos_fetch_queue(const char *name, const aos_type_sig *sig);
-// Same as above, except sets up the returned queue so that it will put messages
-// on *recycle (retrieved with recycle_sig) when they are freed (after they have
-// been released by all other readers/writers and are not in the queue).
-// The length of recycle_sig determines how many freed messages will be kept.
-// Other code can retrieve recycle_sig and sig separately. However, any frees
-// made using aos_fetch_queue with only sig before the recycle queue has been
-// associated with it will not go on to the recyce queue.
-// Will return NULL for both queues if sig->length != recycle_sig->length or
-// sig->hash == recycle_sig->hash (just to be safe).
-// NOTE: calling this function with the same sig but multiple recycle_sig s
-// will result in each freed message being put onto an undefined recycle_sig.
-aos_queue *aos_fetch_queue_recycle(const char *name, const aos_type_sig *sig,
- const aos_type_sig *recycle_sig, aos_queue **recycle);
+ // Constants for passing to options arguments.
+ // The non-conflicting ones can be combined with bitwise-or.
-// Constants for passing to opts arguments.
-// #defines so that c code can use queues
-// The non-conflicting ones can be combined with bitwise-or.
-// TODO(brians) prefix these?
-//
-// Causes the returned message to be left in the queue.
-// For reading only.
-#define PEEK 0x0001
-// Reads the last message in the queue instead of just the next one.
-// NOTE: This removes all of the messages until the last one from the queue
-// (which means that nobody else will read them). However, PEEK means to not
-// remove any from the queue, including the ones that are skipped.
-// For reading only.
-#define FROM_END 0x0002
-// Causes reads to return NULL and writes to fail instead of waiting.
-// For reading and writing.
-#define NON_BLOCK 0x0004
-// Causes things to block.
-// IMPORTANT: #defined to 0 so that it is the default. This has to stay.
-// For reading and writing.
-#define BLOCK 0x0000
-// Causes writes to overwrite the oldest message in the queue instead of
-// blocking.
-// For writing only.
-#define OVERRIDE 0x0008
+ // Causes the returned message to be left in the queue.
+ // For reading only.
+ static const int kPeek = 0x0001;
+ // Reads the last message in the queue instead of just the next one.
+ // NOTE: This removes all of the messages until the last one from the queue
+ // (which means that nobody else will read them). However, PEEK means to not
+ // remove any from the queue, including the ones that are skipped.
+ // For reading only.
+ static const int kFromEnd = 0x0002;
+ // Causes reads to return NULL and writes to fail instead of waiting.
+ // For reading and writing.
+ static const int kNonBlock = 0x0004;
+ // Causes things to block.
+ // IMPORTANT: Has a value of 0 so that it is the default. This has to stay.
+ // For reading and writing.
+ static const int kBlock = 0x0000;
+ // Causes writes to overwrite the oldest message in the queue instead of
+ // blocking.
+ // For writing only.
+ static const int kOverride = 0x0008;
-// Frees a message. Does nothing if msg is NULL.
-int aos_queue_free_msg(aos_queue *queue, const void *msg);
+ // Writes a message into the queue.
+ // This function takes ownership of msg.
+ // NOTE: msg must point to a valid message from this queue
+ // Returns truen on success.
+ bool WriteMessage(void *msg, int options);
-// Writes a message into the queue.
-// NOTE: msg must point to at least the length of this queue's worth of valid
-// data to write
-// IMPORTANT: if this returns -1, then the caller must do something with msg
-// (like free it)
-int aos_queue_write_msg(aos_queue *queue, void *msg, int opts);
-// Exactly the same as aos_queue_write_msg, except it automatically frees the
-// message if writing fails.
-static inline int aos_queue_write_msg_free(aos_queue *queue, void *msg, int opts) {
- const int ret = aos_queue_write_msg(queue, msg, opts);
- if (ret != 0) {
- aos_queue_free_msg(queue, msg);
+ // Reads a message out of the queue.
+ // The return value will have at least the length of this queue's worth of
+ // valid data where it's pointing to.
+ // The return value is const because other people might be viewing the same
+ // messsage. Do not cast the const away!
+ // IMPORTANT: The return value (if not NULL) must eventually be passed to
+ // FreeMessage.
+ const void *ReadMessage(int options);
+ // Exactly the same as aos_queue_read_msg, except it will never return the
+ // same message twice with the same index argument. However, it may not
+ // return some messages that pass through the queue.
+ // *index should start as 0. index does not have to be in shared memory, but
+ // it can be
+ const void *ReadMessageIndex(int options, int *index);
+
+ // Retrieves ("allocates") a message that can then be written to the queue.
+ // NOTE: the return value will be completely uninitialized
+ // The return value will have at least the length of this queue's worth of
+ // valid memory where it's pointing to.
+ // Returns NULL for error.
+ // IMPORTANT: The return value (if not NULL) must eventually be passed to
+ // FreeMessage.
+ void *GetMessage();
+
+ // It is ok to call this method with a NULL msg.
+ void FreeMessage(const void *msg) {
+ if (msg != NULL) DecrementMessageReferenceCount(msg);
}
- return ret;
-}
-// Reads a message out of the queue.
-// The return value will have at least the length of this queue's worth of valid
-// data where it's pointing to.
-// The return value is const because other people might be viewing the same
-// messsage. Do not cast the const away!
-// IMPORTANT: The return value (if not NULL) must eventually be passed to
-// aos_queue_free_msg.
-const void *aos_queue_read_msg(aos_queue *buf, int opts);
-// Exactly the same as aos_queue_read_msg, except it will never return the same
-// message twice with the same index argument. However, it may not return some
-// messages that pass through the queue.
-// *index should start as 0. index does not have to be in shared memory, but it
-// can be
-const void *aos_queue_read_msg_index(aos_queue *queue, int opts, int *index);
+ private:
+ struct MessageHeader;
+ struct ReadData;
-// Retrieves ("allocates") a message that can then be written to the queue.
-// NOTE: the return value will be completely uninitialized
-// The return value will have at least the length of this queue's worth of valid
-// data where it's pointing to.
-// Returns NULL for error.
-// IMPORTANT: The return value (if not NULL) must eventually be passed to
-// aos_queue_free_msg.
-void *aos_queue_get_msg(aos_queue *queue);
+ bool is_readable() { return data_end_ != data_start_; }
+ bool is_writable() { return ((data_end_ + 1) % data_length_) != data_start_; }
-#ifdef __cplusplus
-}
-#endif
+ // These next 4 allow finding the right one.
+ const char *name_;
+ size_t length_;
+ int hash_;
+ int queue_length_;
+ // The next one in the linked list of queues.
+ RawQueue *next_;
-#endif
+ RawQueue *recycle_;
+ Mutex data_lock_; // protects operations on data_ etc
+ // Always gets broadcasted to because different readers might have different
+ // ideas of what "readable" means (ie ones using separated indices).
+ Condition readable_;
+ Condition writable_;
+ int data_length_; // max length into data + 1
+ int data_start_; // is an index into data
+ int data_end_; // is an index into data
+ int messages_; // that have passed through
+ void **data_; // array of messages (with headers)
+
+ Mutex pool_lock_;
+ size_t msg_length_; // sizeof(each message) including the header
+ int mem_length_; // the max number of messages that will ever be allocated
+ int messages_used_;
+ int pool_length_; // the number of allocated messages
+ MessageHeader **pool_; // array of pointers to messages
+
+ // Actually frees the given message.
+ void DoFreeMessage(const void *msg);
+ // Calls DoFreeMessage if appropriate.
+ void DecrementMessageReferenceCount(const void *msg);
+
+ // Should be called with data_lock_ locked.
+ // *read_data will be initialized.
+ // Returns with a readable message in data_ or false.
+ bool ReadCommonStart(int options, int *index, ReadData *read_data);
+ // Deals with setting/unsetting readable_ and writable_.
+ // Should be called after data_lock_ has been unlocked.
+ // read_data should be the same thing that was passed in to ReadCommonStart.
+ void ReadCommonEnd(ReadData *read_data);
+ // Handles reading with kPeek.
+ void *ReadPeek(int options, int start);
+
+ // Gets called by Fetch when necessary (with placement new).
+ RawQueue(const char *name, size_t length, int hash, int queue_length);
+};
+
+} // namespace aos
+
+#endif // AOS_ATOM_CODE_IPC_LIB_QUEUE_H_
diff --git a/aos/atom_code/ipc_lib/queue_internal.h b/aos/atom_code/ipc_lib/queue_internal.h
deleted file mode 100644
index e6b23ef..0000000
--- a/aos/atom_code/ipc_lib/queue_internal.h
+++ /dev/null
@@ -1,62 +0,0 @@
-#ifndef AOS_IPC_LIB_QUEUE_INTERNAL_H_
-#define AOS_IPC_LIB_QUEUE_INTERNAL_H_
-
-#include "shared_mem.h"
-#include "aos_sync.h"
-
-// Should only be used by queue.c. Contains definitions of the structures
-// it uses.
-
-// The number of extra messages the pool associated with each queue will be able
-// to hold (for readers who are slow about freeing them).
-#define EXTRA_MESSAGES 20
-
-typedef struct aos_msg_header_t {
- int ref_count;
- int index; // in the pool
-} aos_msg_header;
-static inline void header_swap(aos_msg_header *l, aos_msg_header *r) {
- aos_msg_header tmp;
- tmp.ref_count = l->ref_count;
- tmp.index = l->index;
- l->ref_count = r->ref_count;
- l->index = r->index;
- r->ref_count = tmp.ref_count;
- r->index = tmp.index;
-}
-
-struct aos_queue_list_t {
- char *name;
- aos_type_sig sig;
- aos_queue *queue;
- aos_queue_list *next;
-};
-
-typedef struct aos_ring_buf_t {
- mutex buff_lock; // the main lock protecting operations on this buffer
- // conditions
- mutex writable;
- mutex readable;
- int length; // max index into data + 1
- int start; // is an index into data
- int end; // is an index into data
- int msgs; // that have passed through
- void **data; // array of messages (w/ headers)
-} aos_ring_buf;
-
-typedef struct aos_msg_pool_t {
- mutex pool_lock;
- size_t msg_length;
- int mem_length; // the number of messages
- int used; // number of messages
- int length; // number of allocated messages
- void **pool; // array of messages
-} aos_msg_pool;
-
-struct aos_queue_t {
- aos_msg_pool pool;
- aos_ring_buf buf;
- aos_queue *recycle;
-};
-
-#endif
diff --git a/aos/atom_code/ipc_lib/queue_test.cpp b/aos/atom_code/ipc_lib/queue_test.cc
similarity index 61%
rename from aos/atom_code/ipc_lib/queue_test.cpp
rename to aos/atom_code/ipc_lib/queue_test.cc
index e9ac8d5..9e5f3ae 100644
--- a/aos/atom_code/ipc_lib/queue_test.cpp
+++ b/aos/atom_code/ipc_lib/queue_test.cc
@@ -1,3 +1,5 @@
+#include "aos/common/queue.h"
+
#include <unistd.h>
#include <sys/mman.h>
#include <inttypes.h>
@@ -8,25 +10,25 @@
#include "gtest/gtest.h"
-#include "aos/atom_code/ipc_lib/sharedmem_test_setup.h"
+#include "aos/atom_code/ipc_lib/core_lib.h"
#include "aos/common/type_traits.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/time.h"
+#include "aos/common/logging/logging.h"
-using testing::AssertionResult;
-using testing::AssertionSuccess;
-using testing::AssertionFailure;
+using ::testing::AssertionResult;
+using ::testing::AssertionSuccess;
+using ::testing::AssertionFailure;
+using ::aos::common::testing::GlobalCoreInstance;
-// IMPORTANT: Some of the functions that do test predicate functions allocate
-// shared memory (and don't free it).
-class QueueTest : public SharedMemTestSetup {
+namespace aos {
+namespace testing {
+
+class QueueTest : public ::testing::Test {
protected:
static const size_t kFailureSize = 400;
static char *fatal_failure;
private:
- // This gets registered right after the fork, so it will get run before any
- // exit handlers that had already been registered.
- static void ExitExitHandler() {
- _exit(EXIT_SUCCESS);
- }
enum class ResultType : uint8_t {
NotCalled,
Called,
@@ -44,32 +46,37 @@
return std::string("unknown(" + static_cast<uint8_t>(result)) + ")";
}
}
- static_assert(aos::shm_ok<ResultType>::value, "this will get put in shared memory");
- // Gets allocated in shared memory so it has to be volatile.
- template<typename T> struct FunctionToCall {
- ResultType result;
+ static_assert(aos::shm_ok<ResultType>::value,
+ "this will get put in shared memory");
+ template<typename T>
+ struct FunctionToCall {
+ FunctionToCall() : result(ResultType::NotCalled) {
+ started.Lock();
+ }
+
+ volatile ResultType result;
bool expected;
void (*function)(T*, char*);
T *arg;
volatile char failure[kFailureSize];
+ Mutex started;
};
- template<typename T> static void Hangs_(volatile FunctionToCall<T> *const to_call) {
+ template<typename T>
+ static void Hangs_(FunctionToCall<T> *const to_call) {
+ to_call->started.Unlock();
to_call->result = ResultType::Called;
to_call->function(to_call->arg, const_cast<char *>(to_call->failure));
to_call->result = ResultType::Returned;
}
- static const long kMsToNs = 1000000;
- // The number of ms after which a function is considered to have hung.
- // Must be < 1000.
- static const long kHangTime = 10;
- static const unsigned int kForkSleep = 0; // how many seconds to sleep after forking
+ // How long until a function is considered to have hung.
+ static constexpr time::Time kHangTime = time::Time::InSeconds(0.035);
+ // How long to sleep after forking (for debugging).
+ static constexpr time::Time kForkSleep = time::Time::InSeconds(0);
// Represents a process that has been forked off. The destructor kills the
// process and wait(2)s for it.
class ForkedProcess {
- const pid_t pid_;
- mutex *const lock_;
public:
ForkedProcess(pid_t pid, mutex *lock) : pid_(pid), lock_(lock) {};
~ForkedProcess() {
@@ -84,24 +91,25 @@
}
const pid_t ret = wait(NULL);
if (ret == -1) {
- fprintf(stderr, "wait(NULL) failed."
- " child %jd might still be alive\n",
- static_cast<intmax_t>(pid_));
+ LOG(WARNING, "wait(NULL) failed."
+ " child %jd might still be alive\n",
+ static_cast<intmax_t>(pid_));
} else if (ret == 0) {
- fprintf(stderr, "child %jd wasn't waitable. it might still be alive\n",
- static_cast<intmax_t>(pid_));
+ LOG(WARNING, "child %jd wasn't waitable. it might still be alive\n",
+ static_cast<intmax_t>(pid_));
} else if (ret != pid_) {
- fprintf(stderr, "child %d is dead, but child %jd might still be alive\n",
- ret, static_cast<intmax_t>(pid_));
+ LOG(WARNING, "child %d is now confirmed dead"
+ ", but child %jd might still be alive\n",
+ ret, static_cast<intmax_t>(pid_));
}
}
enum class JoinResult {
Finished, Hung, Error
};
- JoinResult Join(long timeout = kHangTime) {
- timespec ts{kForkSleep, timeout * kMsToNs};
- switch (mutex_lock_timeout(lock_, &ts)) {
+ JoinResult Join(time::Time timeout = kHangTime) {
+ timespec lock_timeout = (kForkSleep + timeout).ToTimespec();
+ switch (mutex_lock_timeout(lock_, &lock_timeout)) {
case 2:
return JoinResult::Hung;
case 0:
@@ -110,9 +118,13 @@
return JoinResult::Error;
}
}
+
+ private:
+ const pid_t pid_;
+ mutex *const lock_;
} __attribute__((unused));
- // Member variables for HangsFork and HangsCheck.
+ // State for HangsFork and HangsCheck.
typedef uint8_t ChildID;
static void ReapExitHandler() {
for (auto it = children_.begin(); it != children_.end(); ++it) {
@@ -120,11 +132,11 @@
}
}
static std::map<ChildID, ForkedProcess *> children_;
- std::map<ChildID, volatile FunctionToCall<void> *> to_calls_;
+ std::map<ChildID, FunctionToCall<void> *> to_calls_;
- void SetUp() {
- SharedMemTestSetup::SetUp();
- fatal_failure = reinterpret_cast<char *>(shm_malloc(sizeof(fatal_failure)));
+ void SetUp() override {
+ ::testing::Test::SetUp();
+ fatal_failure = static_cast<char *>(shm_malloc(sizeof(fatal_failure)));
static bool registered = false;
if (!registered) {
atexit(ReapExitHandler);
@@ -133,30 +145,30 @@
}
protected:
- // Function gets called with arg in a forked process.
+ // function gets called with arg in a forked process.
// Leaks shared memory.
- // the attribute is in the middle to make gcc happy
template<typename T> __attribute__((warn_unused_result))
- std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
- mutex *lock = reinterpret_cast<mutex *>(shm_malloc_aligned(
+ std::unique_ptr<ForkedProcess> ForkExecute(void (*function)(T*), T *arg) {
+ mutex *lock = static_cast<mutex *>(shm_malloc_aligned(
sizeof(*lock), sizeof(int)));
- *lock = 1;
+ assert(mutex_lock(lock) == 0);
const pid_t pid = fork();
switch (pid) {
- case 0: // child
- if (kForkSleep != 0) {
- printf("pid %jd sleeping for %u\n", static_cast<intmax_t>(getpid()),
- kForkSleep);
- sleep(kForkSleep);
+ case 0: // child
+ if (kForkSleep != time::Time(0, 0)) {
+ LOG(INFO, "pid %jd sleeping for %ds%dns\n",
+ static_cast<intmax_t>(getpid()),
+ kForkSleep.sec(), kForkSleep.nsec());
+ time::SleepFor(kForkSleep);
}
- atexit(ExitExitHandler);
+ ::aos::common::testing::PreventExit();
function(arg);
mutex_unlock(lock);
exit(EXIT_SUCCESS);
- case -1: // parent failure
- printf("fork() failed with %d: %s\n", errno, strerror(errno));
+ case -1: // parent failure
+ LOG(ERROR, "fork() failed with %d: %s\n", errno, strerror(errno));
return std::unique_ptr<ForkedProcess>();
- default: // parent
+ default: // parent
return std::unique_ptr<ForkedProcess>(new ForkedProcess(pid, lock));
}
}
@@ -166,8 +178,8 @@
// NOTE: There are other reasons for it to return a failure than the function
// doing the wrong thing.
// Leaks shared memory.
- template<typename T> AssertionResult Hangs(void (*function)(T*, char*), T *arg,
- bool expected) {
+ template<typename T>
+ AssertionResult Hangs(void (*function)(T*, char*), T *arg, bool expected) {
AssertionResult fork_result(HangsFork<T>(function, arg, expected, 0));
if (!fork_result) {
return fork_result;
@@ -178,21 +190,24 @@
// Use HangsCheck to get the result.
// Returns whether the fork succeeded or not, NOT whether or not the hang
// check succeeded.
- template<typename T> AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
- bool expected, ChildID id) {
+ template<typename T>
+ AssertionResult HangsFork(void (*function)(T*, char *), T *arg,
+ bool expected, ChildID id) {
static_assert(aos::shm_ok<FunctionToCall<T>>::value,
"this is going into shared memory");
- volatile FunctionToCall<T> *const to_call = reinterpret_cast<FunctionToCall<T> *>(
- shm_malloc_aligned(sizeof(*to_call), sizeof(int)));
- to_call->result = ResultType::NotCalled;
+ FunctionToCall<T> *const to_call =
+ static_cast<FunctionToCall<T> *>(
+ shm_malloc_aligned(sizeof(*to_call), alignof(FunctionToCall<T>)));
+ new (to_call) FunctionToCall<T>();
to_call->function = function;
to_call->arg = arg;
to_call->expected = expected;
to_call->failure[0] = '\0';
- static_cast<volatile char *>(fatal_failure)[0] = '\0';
+ static_cast<char *>(fatal_failure)[0] = '\0';
children_[id] = ForkExecute(Hangs_, to_call).release();
if (!children_[id]) return AssertionFailure() << "ForkExecute failed";
- to_calls_[id] = reinterpret_cast<volatile FunctionToCall<void> *>(to_call);
+ to_calls_[id] = reinterpret_cast<FunctionToCall<void> *>(to_call);
+ to_call->started.Lock();
return AssertionSuccess();
}
// Checks whether or not a function hung like it was supposed to.
@@ -214,8 +229,12 @@
<< ResultTypeString(to_calls_[id]->result));
} else {
if (to_calls_[id]->result == ResultType::Called) {
- return to_calls_[id]->expected ? AssertionSuccess() : AssertionFailure();
+ return to_calls_[id]->expected ? AssertionSuccess() :
+ AssertionFailure();
+ } else if (result == ForkedProcess::JoinResult::Error) {
+ return AssertionFailure() << "error joining child";
} else {
+ abort();
return AssertionFailure() << "something weird happened";
}
}
@@ -234,28 +253,30 @@
} while (false)
struct TestMessage {
- int16_t data; // don't really want to test empty messages
+ // Some contents because we don't really want to test empty messages.
+ int16_t data;
};
struct MessageArgs {
- aos_queue *const queue;
+ RawQueue *const queue;
int flags;
- int16_t data; // -1 means NULL expected
+ int16_t data; // -1 means NULL expected
};
static void WriteTestMessage(MessageArgs *args, char *failure) {
- TestMessage *msg = reinterpret_cast<TestMessage *>(aos_queue_get_msg(args->queue));
+ TestMessage *msg = static_cast<TestMessage *>(args->queue->GetMessage());
if (msg == NULL) {
- snprintf(fatal_failure, kFailureSize, "couldn't get_msg from %p", args->queue);
+ snprintf(fatal_failure, kFailureSize,
+ "couldn't get_msg from %p", args->queue);
return;
}
msg->data = args->data;
- if (aos_queue_write_msg_free(args->queue, msg, args->flags) == -1) {
+ if (!args->queue->WriteMessage(msg, args->flags)) {
snprintf(failure, kFailureSize, "write_msg_free(%p, %p, %d) failed",
args->queue, msg, args->flags);
}
}
static void ReadTestMessage(MessageArgs *args, char *failure) {
- const TestMessage *msg = reinterpret_cast<const TestMessage *>(
- aos_queue_read_msg(args->queue, args->flags));
+ const TestMessage *msg = static_cast<const TestMessage *>(
+ args->queue->ReadMessage(args->flags));
if (msg == NULL) {
if (args->data != -1) {
snprintf(failure, kFailureSize,
@@ -268,89 +289,88 @@
"expected data of %" PRId16 " but got %" PRId16 " instead",
args->data, msg->data);
}
- aos_queue_free_msg(args->queue, msg);
+ args->queue->FreeMessage(msg);
}
}
+
+ private:
+ GlobalCoreInstance my_core;
};
char *QueueTest::fatal_failure;
std::map<QueueTest::ChildID, QueueTest::ForkedProcess *> QueueTest::children_;
+constexpr time::Time QueueTest::kHangTime;
+constexpr time::Time QueueTest::kForkSleep;
TEST_F(QueueTest, Reading) {
- static const aos_type_sig signature{sizeof(TestMessage), 1, 1};
- aos_queue *const queue = aos_fetch_queue("Queue", &signature);
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
MessageArgs args{queue, 0, -1};
- EXPECT_EQ(BLOCK, 0);
- EXPECT_EQ(BLOCK | FROM_END, FROM_END);
-
- args.flags = NON_BLOCK;
+ args.flags = RawQueue::kNonBlock;
EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = NON_BLOCK | PEEK;
+ args.flags = RawQueue::kNonBlock | RawQueue::kPeek;
EXPECT_RETURNS(ReadTestMessage, &args);
args.flags = 0;
EXPECT_HANGS(ReadTestMessage, &args);
- args.flags = PEEK;
+ args.flags = RawQueue::kPeek;
EXPECT_HANGS(ReadTestMessage, &args);
args.data = 254;
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = PEEK;
+ args.flags = RawQueue::kPeek;
EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = PEEK;
+ args.flags = RawQueue::kPeek;
EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = PEEK | NON_BLOCK;
+ args.flags = RawQueue::kPeek | RawQueue::kNonBlock;
EXPECT_RETURNS(ReadTestMessage, &args);
args.flags = 0;
EXPECT_RETURNS(ReadTestMessage, &args);
args.flags = 0;
args.data = -1;
EXPECT_HANGS(ReadTestMessage, &args);
- args.flags = NON_BLOCK;
+ args.flags = RawQueue::kNonBlock;
EXPECT_RETURNS(ReadTestMessage, &args);
args.flags = 0;
args.data = 971;
EXPECT_RETURNS_FAILS(ReadTestMessage, &args);
}
TEST_F(QueueTest, Writing) {
- static const aos_type_sig signature{sizeof(TestMessage), 1, 1};
- aos_queue *const queue = aos_fetch_queue("Queue", &signature);
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
MessageArgs args{queue, 0, 973};
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
EXPECT_HANGS(WriteTestMessage, &args);
- args.flags = NON_BLOCK;
+ args.flags = RawQueue::kNonBlock;
EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
- args.flags = NON_BLOCK;
+ args.flags = RawQueue::kNonBlock;
EXPECT_RETURNS_FAILS(WriteTestMessage, &args);
- args.flags = PEEK;
+ args.flags = RawQueue::kPeek;
EXPECT_RETURNS(ReadTestMessage, &args);
args.data = 971;
- args.flags = OVERRIDE;
+ args.flags = RawQueue::kOverride;
EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = OVERRIDE;
+ args.flags = RawQueue::kOverride;
EXPECT_RETURNS(WriteTestMessage, &args);
args.flags = 0;
EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = NON_BLOCK;
+ args.flags = RawQueue::kNonBlock;
EXPECT_RETURNS(WriteTestMessage, &args);
args.flags = 0;
EXPECT_RETURNS(ReadTestMessage, &args);
- args.flags = OVERRIDE;
+ args.flags = RawQueue::kOverride;
EXPECT_RETURNS(WriteTestMessage, &args);
args.flags = 0;
EXPECT_RETURNS(ReadTestMessage, &args);
}
TEST_F(QueueTest, MultiRead) {
- static const aos_type_sig signature{sizeof(TestMessage), 1, 1};
- aos_queue *const queue = aos_fetch_queue("Queue", &signature);
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage), 1, 1);
MessageArgs args{queue, 0, 1323};
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
EXPECT_RETURNS(WriteTestMessage, &args);
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 1));
ASSERT_TRUE(HangsFork(ReadTestMessage, &args, true, 2));
EXPECT_TRUE(HangsCheck(1) != HangsCheck(2));
@@ -360,45 +380,45 @@
TEST_F(QueueTest, Recycle) {
// TODO(brians) basic test of recycle queue
// include all of the ways a message can get into the recycle queue
- static const aos_type_sig signature{sizeof(TestMessage), 1, 2},
- recycle_signature{sizeof(TestMessage), 2, 2};
- aos_queue *recycle_queue = reinterpret_cast<aos_queue *>(23);
- aos_queue *const queue = aos_fetch_queue_recycle("Queue", &signature,
- &recycle_signature, &recycle_queue);
- ASSERT_NE(reinterpret_cast<aos_queue *>(23), recycle_queue);
+ RawQueue *recycle_queue = reinterpret_cast<RawQueue *>(23);
+ RawQueue *const queue = RawQueue::Fetch("Queue", sizeof(TestMessage),
+ 1, 2, 2, 2, &recycle_queue);
+ ASSERT_NE(reinterpret_cast<RawQueue *>(23), recycle_queue);
MessageArgs args{queue, 0, 973}, recycle{recycle_queue, 0, 973};
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
EXPECT_RETURNS(WriteTestMessage, &args);
EXPECT_HANGS(ReadTestMessage, &recycle);
args.data = 254;
EXPECT_RETURNS(WriteTestMessage, &args);
EXPECT_HANGS(ReadTestMessage, &recycle);
args.data = 971;
- args.flags = OVERRIDE;
+ args.flags = RawQueue::kOverride;
EXPECT_RETURNS(WriteTestMessage, &args);
- recycle.flags = BLOCK;
+ recycle.flags = RawQueue::kBlock;
EXPECT_RETURNS(ReadTestMessage, &recycle);
EXPECT_HANGS(ReadTestMessage, &recycle);
- TestMessage *msg = static_cast<TestMessage *>(aos_queue_get_msg(queue));
+ TestMessage *msg = static_cast<TestMessage *>(queue->GetMessage());
ASSERT_TRUE(msg != NULL);
msg->data = 341;
- aos_queue_free_msg(queue, msg);
+ queue->FreeMessage(msg);
recycle.data = 341;
EXPECT_RETURNS(ReadTestMessage, &recycle);
EXPECT_HANGS(ReadTestMessage, &recycle);
args.data = 254;
- args.flags = PEEK;
+ args.flags = RawQueue::kPeek;
EXPECT_RETURNS(ReadTestMessage, &args);
- recycle.flags = BLOCK;
+ recycle.flags = RawQueue::kBlock;
EXPECT_HANGS(ReadTestMessage, &recycle);
- args.flags = BLOCK;
+ args.flags = RawQueue::kBlock;
EXPECT_RETURNS(ReadTestMessage, &args);
recycle.data = 254;
EXPECT_RETURNS(ReadTestMessage, &recycle);
}
+} // namespace testing
+} // namespace aos
diff --git a/aos/atom_code/ipc_lib/shared_mem.c b/aos/atom_code/ipc_lib/shared_mem.c
index f631b78..e2c2c9e 100644
--- a/aos/atom_code/ipc_lib/shared_mem.c
+++ b/aos/atom_code/ipc_lib/shared_mem.c
@@ -1,4 +1,4 @@
-#include "shared_mem.h"
+#include "aos/atom_code/ipc_lib/shared_mem.h"
#include <stdio.h>
#include <string.h>
@@ -8,22 +8,31 @@
#include <sys/types.h>
#include <errno.h>
+#include "aos/atom_code/ipc_lib/core_lib.h"
+
// the path for the shared memory segment. see shm_open(3) for restrictions
#define AOS_SHM_NAME "/aos_shared_mem"
// Size of the shared mem segment.
// set to the maximum number that worked
#define SIZEOFSHMSEG (4096 * 27813)
+void init_shared_mem_core(aos_shm_core *shm_core) {
+ clock_gettime(CLOCK_REALTIME, &shm_core->identifier);
+ shm_core->msg_alloc_lock = 0;
+ shm_core->queues.queue_list = NULL;
+ shm_core->queues.alloc_lock = 0;
+}
+
ptrdiff_t aos_core_get_mem_usage(void) {
return global_core->size -
((ptrdiff_t)global_core->mem_struct->msg_alloc -
(ptrdiff_t)global_core->mem_struct);
}
-struct aos_core global_core_data;
struct aos_core *global_core = NULL;
int aos_core_create_shared_mem(enum aos_core_create to_create) {
+ static struct aos_core global_core_data;
global_core = &global_core_data;
int shm;
before:
@@ -46,8 +55,8 @@
}
if (shm == -1) {
fprintf(stderr, "shared_mem:"
- " shm_open(" AOS_SHM_NAME ", O_RDWR [| O_CREAT | O_EXCL, 0|0666)"
- " failed with %d: %s\n", errno, strerror(errno));
+ " shm_open(" AOS_SHM_NAME ", O_RDWR [| O_CREAT | O_EXCL, 0|0666)"
+ " failed with %d: %s\n", errno, strerror(errno));
return -1;
}
if (global_core->owner) {
@@ -62,8 +71,8 @@
MAP_SHARED | MAP_FIXED | MAP_LOCKED | MAP_POPULATE, shm, 0);
if (shm_address == MAP_FAILED) {
fprintf(stderr, "shared_mem: mmap(%p, 0x%zx, stuff, stuff, %d, 0) failed"
- " with %d: %s\n",
- (void *)SHM_START, SIZEOFSHMSEG, shm, errno, strerror(errno));
+ " with %d: %s\n",
+ (void *)SHM_START, SIZEOFSHMSEG, shm, errno, strerror(errno));
return -1;
}
printf("shared_mem: shm at: %p\n", shm_address);
@@ -88,9 +97,9 @@
init_shared_mem_core(global_core->mem_struct);
}
if (global_core->owner) {
- condition_set(&global_core->mem_struct->creation_condition);
+ futex_set(&global_core->mem_struct->creation_condition);
} else {
- if (condition_wait(&global_core->mem_struct->creation_condition) != 0) {
+ if (futex_wait(&global_core->mem_struct->creation_condition) != 0) {
fprintf(stderr, "waiting on creation_condition failed\n");
return -1;
}
@@ -116,4 +125,3 @@
}
return 0;
}
-
diff --git a/aos/atom_code/ipc_lib/shared_mem.h b/aos/atom_code/ipc_lib/shared_mem.h
index b1a2608..c0d21ac 100644
--- a/aos/atom_code/ipc_lib/shared_mem.h
+++ b/aos/atom_code/ipc_lib/shared_mem.h
@@ -1,19 +1,40 @@
#ifndef _SHARED_MEM_H_
#define _SHARED_MEM_H_
+#include <stddef.h>
+#include <unistd.h>
+#include <time.h>
+
+#include "aos/atom_code/ipc_lib/aos_sync.h"
+
#ifdef __cplusplus
extern "C" {
#endif
-#include "core_lib.h"
-#include <stddef.h>
-#include <unistd.h>
+extern struct aos_core *global_core;
// Where the shared memory segment starts in each process's address space.
// Has to be the same in all of them so that stuff in shared memory
// can have regular pointers to other stuff in shared memory.
#define SHM_START 0x20000000
+typedef struct aos_queue_global_t {
+ mutex alloc_lock;
+ void *queue_list; // an aos::Queue* declared in C code
+} aos_queue_global;
+
+typedef struct aos_shm_core_t {
+ // clock_gettime(CLOCK_REALTIME, &identifier) gets called to identify
+ // this shared memory area
+ struct timespec identifier;
+ // gets 0-initialized at the start (as part of shared memory) and
+ // the owner sets as soon as it finishes setting stuff up
+ mutex creation_condition;
+ mutex msg_alloc_lock;
+ void *msg_alloc;
+ aos_queue_global queues;
+} aos_shm_core;
+
enum aos_core_create {
create,
reference
@@ -26,6 +47,8 @@
aos_shm_core *mem_struct;
};
+void init_shared_mem_core(aos_shm_core *shm_core);
+
ptrdiff_t aos_core_get_mem_usage(void);
// Takes the specified memory address and uses it as the shared memory.
diff --git a/aos/atom_code/ipc_lib/sharedmem_test_setup.h b/aos/atom_code/ipc_lib/sharedmem_test_setup.h
deleted file mode 100644
index e461c43..0000000
--- a/aos/atom_code/ipc_lib/sharedmem_test_setup.h
+++ /dev/null
@@ -1,147 +0,0 @@
-// defines a fixture (SharedMemTestSetup) that sets up shared memory
-
-extern "C" {
-#include "shared_mem.h"
- extern struct aos_core *global_core;
-}
-
-#include <signal.h>
-
-#include <gtest/gtest.h>
-#include <sys/types.h>
-
-// TODO(brians) read logs from here
-class SharedMemTestSetup : public testing::Test{
- protected:
- pid_t core;
- int start[2];
- int memcheck[2];
- static void signal_handler(int){
- if(aos_core_free_shared_mem()){
- exit(- 1);
- }
- exit(0);
- }
- static int get_mem_usage(){
- return global_core->size - ((uint8_t *)global_core->mem_struct->msg_alloc - (uint8_t *)SHM_START);
- }
- bool checking_mem;
-
- virtual void BeforeLocalShmSetup() {}
- virtual void SetUp(){
- ASSERT_EQ(0, pipe(start)) << "couldn't create start pipes";
- ASSERT_EQ(0, pipe(memcheck)) << "couldn't create memcheck pipes";
- checking_mem = false;
- if((core = fork()) == 0){
- close(start[0]);
- close(memcheck[1]);
- struct sigaction act;
- act.sa_handler = signal_handler;
- sigaction(SIGINT, &act, NULL);
- if(aos_core_create_shared_mem(create)){
- exit(-1);
- }
- write_pipe(start[1], "a", 1);
- int usage = 0;
- while(1){
- char buf1;
- read_pipe(memcheck[0], &buf1, 1);
- if(usage == 0)
- usage = get_mem_usage();
- if(usage == get_mem_usage())
- buf1 = 1;
- else
- buf1 = 0;
- write_pipe(start[1], &buf1, 1);
- }
- }
- close(start[1]);
- close(memcheck[0]);
- ASSERT_NE(-1, core) << "fork failed";
- char buf;
- read_pipe(start[0], &buf, 1);
-
- BeforeLocalShmSetup();
-
- ASSERT_EQ(0, aos_core_create_shared_mem(reference)) << "couldn't create shared mem reference";
- }
- virtual void TearDown(){
- if(checking_mem){
- write_pipe(memcheck[1], "a", 1);
- char buf;
- read_pipe(start[0], &buf, 1);
- EXPECT_EQ(1, buf) << "memory got leaked";
- }
- EXPECT_EQ(0, aos_core_free_shared_mem()) << "issues freeing shared mem";
- if(core > 0){
- kill(core, SIGINT);
- siginfo_t status;
- ASSERT_EQ(0, waitid(P_PID, core, &status, WEXITED)) << "waiting for the core to finish failed";
- EXPECT_EQ(CLD_EXITED, status.si_code) << "core died";
- EXPECT_EQ(0, status.si_status) << "core exited with an error";
- }
- }
- // if any more shared memory gets allocated after calling this and not freed by the end, it's an error
- void AllSharedMemAllocated(){
- checking_mem = true;
- write_pipe(memcheck[1], "a", 1);
- char buf;
- read_pipe(start[0], &buf, 1);
- }
- private:
- // Wrapper functions for pipes because they should never have errors.
- void read_pipe(int fd, void *buf, size_t count) {
- if (read(fd, buf, count) < 0) abort();
- }
- void write_pipe(int fd, const void *buf, size_t count) {
- if (write(fd, buf, count) < 0) abort();
- }
-};
-class ExecVeTestSetup : public SharedMemTestSetup {
- protected:
- std::vector<std::string> files;
- std::vector<pid_t> pids;
- virtual void BeforeLocalShmSetup(){
- std::vector<std::string>::iterator it;
- pid_t child;
- for(it = files.begin(); it < files.end(); ++it){
- if((child = fork()) == 0){
- char *null = NULL;
- execve(it->c_str(), &null, &null);
- ADD_FAILURE() << "execve failed";
- perror("execve");
- exit(0);
- }
- if(child > 0)
- pids.push_back(child);
- else
- ADD_FAILURE() << "fork failed return=" << child;
- }
- usleep(150000);
- }
- virtual void TearDown(){
- std::vector<pid_t>::iterator it;
- siginfo_t status;
- for(it = pids.begin(); it < pids.end(); ++it){
- printf("attempting to SIGINT(%d) %d\n", SIGINT, *it);
- if(*it > 0){
- kill(*it, SIGINT);
- ASSERT_EQ(0, waitid(P_PID, *it, &status, WEXITED)) << "waiting for the AsyncAction(pid=" << *it << ") to finish failed";
- EXPECT_EQ(CLD_EXITED, status.si_code) << "child died (killed by signal is " << (int)CLD_KILLED << ")";
- EXPECT_EQ(0, status.si_status) << "child exited with an error";
- }else{
- FAIL();
- }
- }
-
- SharedMemTestSetup::TearDown();
- }
- // call this _before_ ExecVeTestSetup::SetUp()
- void AddProcess(const std::string file){
- files.push_back(file);
- }
- void PercolatePause(){
- usleep(50000);
- }
-};
-
diff --git a/aos/atom_code/logging/atom_logging.cc b/aos/atom_code/logging/atom_logging.cc
index 08a65cb..854da17 100644
--- a/aos/atom_code/logging/atom_logging.cc
+++ b/aos/atom_code/logging/atom_logging.cc
@@ -53,8 +53,7 @@
return process_name + '.' + thread_name;
}
-static const aos_type_sig message_sig = {sizeof(LogMessage), 1323, 1500};
-static aos_queue *queue;
+RawQueue *queue;
} // namespace
namespace internal {
@@ -83,7 +82,7 @@
class AtomQueueLogImplementation : public LogImplementation {
virtual void DoLog(log_level level, const char *format, va_list ap) {
- LogMessage *message = static_cast<LogMessage *>(aos_queue_get_msg(queue));
+ LogMessage *message = static_cast<LogMessage *>(queue->GetMessage());
if (message == NULL) {
LOG(FATAL, "queue get message failed\n");
}
@@ -99,7 +98,7 @@
void Register() {
Init();
- queue = aos_fetch_queue("LoggingQueue", &message_sig);
+ queue = RawQueue::Fetch("LoggingQueue", sizeof(LogMessage), 1323, 1500);
if (queue == NULL) {
Die("logging: couldn't fetch queue\n");
}
@@ -108,33 +107,32 @@
}
const LogMessage *ReadNext(int flags, int *index) {
- return static_cast<const LogMessage *>(
- aos_queue_read_msg_index(queue, flags, index));
+ return static_cast<const LogMessage *>(queue->ReadMessageIndex(flags, index));
}
const LogMessage *ReadNext() {
- return ReadNext(BLOCK);
+ return ReadNext(RawQueue::kBlock);
}
const LogMessage *ReadNext(int flags) {
const LogMessage *r = NULL;
do {
- r = static_cast<const LogMessage *>(aos_queue_read_msg(queue, flags));
+ r = static_cast<const LogMessage *>(queue->ReadMessage(flags));
// not blocking means return a NULL if that's what it gets
- } while ((flags & BLOCK) && r == NULL);
+ } while ((flags & RawQueue::kBlock) && r == NULL);
return r;
}
LogMessage *Get() {
- return static_cast<LogMessage *>(aos_queue_get_msg(queue));
+ return static_cast<LogMessage *>(queue->GetMessage());
}
void Free(const LogMessage *msg) {
- aos_queue_free_msg(queue, msg);
+ queue->FreeMessage(msg);
}
void Write(LogMessage *msg) {
- if (aos_queue_write_msg_free(queue, msg, OVERRIDE) < 0) {
+ if (!queue->WriteMessage(msg, RawQueue::kOverride)) {
LOG(FATAL, "writing failed");
}
}
diff --git a/aos/atom_code/queue-tmpl.h b/aos/atom_code/queue-tmpl.h
index bb043e1..0cc9392 100644
--- a/aos/atom_code/queue-tmpl.h
+++ b/aos/atom_code/queue-tmpl.h
@@ -5,7 +5,7 @@
assert(msg_ != NULL);
msg_->SetTimeToNow();
assert(queue_ != NULL);
- bool return_value = aos_queue_write_msg_free(queue_, msg_, OVERRIDE) == 0;
+ bool return_value = queue_->WriteMessage(msg_, RawQueue::kOverride);
msg_ = NULL;
return return_value;
}
@@ -15,7 +15,7 @@
assert(msg_ != NULL);
msg_->SetTimeToNow();
assert(queue_ != NULL);
- bool return_value = aos_queue_write_msg_free(queue_, msg_, BLOCK) == 0;
+ bool return_value = queue_->WriteMessage(msg_, RawQueue::kBlock);
msg_ = NULL;
return return_value;
}
@@ -23,7 +23,7 @@
template <class T>
void ScopedMessagePtr<T>::reset(T *msg) {
if (queue_ != NULL && msg_ != NULL) {
- aos_queue_free_msg(queue_, msg_);
+ queue_->FreeMessage(msg);
}
msg_ = msg;
}
@@ -81,13 +81,12 @@
assert(msg_ != NULL);
assert(queue_ != NULL);
msg_->SetTimeToNow();
- T *shm_msg = static_cast<T *>(aos_queue_get_msg(queue_));
+ T *shm_msg = static_cast<T *>(queue_->GetMessage());
if (shm_msg == NULL) {
return false;
}
*shm_msg = *msg_;
- bool return_value =
- aos_queue_write_msg_free(queue_, shm_msg, OVERRIDE) == 0;
+ bool return_value = queue_->WriteMessage(shm_msg, RawQueue::kOverride);
reset();
return return_value;
}
@@ -100,12 +99,12 @@
assert(msg_ != NULL);
assert(queue_ != NULL);
msg_->SetTimeToNow();
- T *shm_msg = static_cast<T *>(aos_queue_get_msg(queue_));
+ T *shm_msg = static_cast<T *>(queue_->GetMessage());
if (shm_msg == NULL) {
return false;
}
*shm_msg = *msg_;
- bool return_value = aos_queue_write_msg_free(queue_, shm_msg, BLOCK) == 0;
+ bool return_value = queue_->WriteMessage(shm_msg, RawQueue::kBlock);
reset();
return return_value;
}
@@ -145,7 +144,7 @@
friend class aos::SafeMessageBuilder<T>;
// Only Queue should be able to build a message pointer.
- SafeScopedMessagePtr(aos_queue *queue)
+ SafeScopedMessagePtr(RawQueue *queue)
: queue_(queue), msg_(new T()) {}
// Sets the pointer to msg, freeing the old value if it was there.
@@ -159,10 +158,10 @@
}
// Sets the queue that owns this message.
- void set_queue(aos_queue *queue) { queue_ = queue; }
+ void set_queue(RawQueue *queue) { queue_ = queue; }
// The queue that the message is a part of.
- aos_queue *queue_;
+ RawQueue *queue_;
// The message or NULL.
T *msg_;
};
@@ -170,11 +169,9 @@
template <class T>
void Queue<T>::Init() {
if (queue_ == NULL) {
- // Signature of the message.
- aos_type_sig kQueueSignature{sizeof(T), static_cast<int>(T::kHash),
- T::kQueueLength};
-
- queue_ = aos_fetch_queue(queue_name_, &kQueueSignature);
+ queue_ = RawQueue::Fetch(queue_name_, sizeof(T),
+ static_cast<int>(T::kHash),
+ T::kQueueLength);
queue_msg_.set_queue(queue_);
}
}
@@ -191,11 +188,11 @@
template <class T>
bool Queue<T>::FetchNext() {
Init();
- // TODO(aschuh): Use aos_queue_read_msg_index so that multiple readers
+ // TODO(aschuh): Use RawQueue::ReadMessageIndex so that multiple readers
// reading don't randomly get only part of the messages.
// Document here the tradoffs that are part of each method.
- const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_,
- NON_BLOCK));
+ const T *msg = static_cast<const T *>(
+ queue_->ReadMessage(RawQueue::kNonBlock));
// Only update the internal pointer if we got a new message.
if (msg != NULL) {
queue_msg_.reset(msg);
@@ -206,7 +203,7 @@
template <class T>
bool Queue<T>::FetchNextBlocking() {
Init();
- const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_, BLOCK));
+ const T *msg = static_cast<const T *>(queue_->ReadMessage(RawQueue::kBlock));
queue_msg_.reset(msg);
assert (msg != NULL);
return true;
@@ -215,16 +212,16 @@
template <class T>
bool Queue<T>::FetchLatest() {
Init();
- const T *msg = static_cast<const T *>(aos_queue_read_msg(queue_,
- FROM_END | NON_BLOCK | PEEK));
+ const T *msg = static_cast<const T *>(queue_->ReadMessage(
+ RawQueue::kFromEnd | RawQueue::kNonBlock | RawQueue::kPeek));
// Only update the internal pointer if we got a new message.
if (msg != NULL && msg != queue_msg_.get()) {
queue_msg_.reset(msg);
return true;
}
- // The message has to get freed if we didn't use it (and aos_queue_free_msg is
- // ok to call on NULL).
- aos_queue_free_msg(queue_, msg);
+ // The message has to get freed if we didn't use it (and RawQueue::FreeMessage
+ // is ok to call on NULL).
+ queue_->FreeMessage(msg);
return false;
}
@@ -244,7 +241,7 @@
template <class T>
T *Queue<T>::MakeRawMessage() {
- T *ret = static_cast<T *>(aos_queue_get_msg(queue_));
+ T *ret = static_cast<T *>(queue_->GetMessage());
assert(ret != NULL);
return ret;
}
diff --git a/aos/build/aos.gyp b/aos/build/aos.gyp
index 874b6a7..c91ab43 100644
--- a/aos/build/aos.gyp
+++ b/aos/build/aos.gyp
@@ -17,10 +17,10 @@
'<(AOS)/atom_code/logging/atom_logging.cc',
],
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
'export_dependent_settings': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
]
}],
],
diff --git a/aos/build/aos.gypi b/aos/build/aos.gypi
index f6c3126..9c74438 100644
--- a/aos/build/aos.gypi
+++ b/aos/build/aos.gypi
@@ -23,12 +23,8 @@
'conditions': [
['OS=="crio"', {
'make_global_settings': [
- ['CC', '<!(which powerpc-wrs-vxworks-gcc)'],
- ['CXX', '<!(which powerpc-wrs-vxworks-g++)'],
- ['LD', '<!(readlink -f <(AOS)/build/crio_link_out)'],
- #['LD', 'powerpc-wrs-vxworks-ld'],
- #['AR', '<!(which powerpc-wrs-vxworks-ar)'],
- #['NM', '<!(which powerpc-wrs-vxworks-nm)'],
+ ['CC', '<!(readlink -f <(AOS)/build/crio_cc)'],
+ ['CXX', '<!(readlink -f <(AOS)/build/crio_cxx)'],
],
'variables': {
'aos_target': 'static_library',
diff --git a/aos/build/aos_all.gyp b/aos/build/aos_all.gyp
index bda36fa..1aa6063 100644
--- a/aos/build/aos_all.gyp
+++ b/aos/build/aos_all.gyp
@@ -13,16 +13,15 @@
'../atom_code/camera/camera.gyp:CameraHTTPStreamer',
'../atom_code/camera/camera.gyp:CameraReader',
'../atom_code/core/core.gyp:*',
- '../atom_code/ipc_lib/ipc_lib.gyp:*',
+ '../atom_code/ipc_lib/ipc_lib.gyp:raw_queue_test',
+ '../atom_code/ipc_lib/ipc_lib.gyp:ipc_stress_test',
'../atom_code/starter/starter.gyp:starter_exe',
'../atom_code/starter/starter.gyp:netconsole',
'../common/common.gyp:queue_test',
'../common/common.gyp:die_test',
'../common/util/util.gyp:trapezoid_profile_test',
- '../common/sensors/sensors.gyp:sensor_receiver_test',
+ '../common/util/util.gyp:wrapping_counter_test',
'Common',
- # TODO(brians): move this to Common
- '<(AOS)/common/sensors/sensors.gyp:sensors_test',
],
},
{
@@ -42,6 +41,7 @@
'<(AOS)/common/common.gyp:type_traits_test',
'<(AOS)/common/common.gyp:time_test',
'<(AOS)/common/common.gyp:mutex_test',
+ '<(AOS)/common/common.gyp:condition_test',
'<(AOS)/common/common.gyp:once_test',
'<(AOS)/common/logging/logging.gyp:logging_impl_test',
],
diff --git a/aos/build/build.sh b/aos/build/build.sh
index 6afb8bb..8fef47d 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -1,8 +1,10 @@
-#!/bin/bash -e
+#!/bin/bash
#set -x
+set -e
+
# This file should be called to build the code.
-# Usage: build.sh platform main_file.gyp debug [action]
+# Usage: build.sh platform main_file.gyp debug [action]...
PLATFORM=$1
GYP_MAIN=$2
@@ -10,23 +12,27 @@
OUT_NAME=$4
ACTION=$5
+shift 3
+shift || true # We might not have a 4th argument if ACTION is empty.
+
export WIND_BASE=${WIND_BASE:-"/usr/local/powerpc-wrs-vxworks/wind_base"}
-[ ${PLATFORM} == "crio" -o ${PLATFORM} == "atom" ] || ( echo Platform "(${PLATFORM})" must be '"crio" or "atom"'. ; exit 1 )
-[ ${DEBUG} == "yes" -o ${DEBUG} == "no" ] || ( echo Debug "(${DEBUG})" must be '"yes" or "no"'. ; exit 1 )
+[ "${PLATFORM}" == "crio" -o "${PLATFORM}" == "atom" ] || ( echo Platform "(${PLATFORM})" must be '"crio" or "atom"'. ; exit 1 )
+[ "${DEBUG}" == "yes" -o "${DEBUG}" == "no" ] || ( echo Debug "(${DEBUG})" must be '"yes" or "no"'. ; exit 1 )
AOS=`dirname $0`/..
-NINJA_DIR=${AOS}/externals/ninja
+NINJA_RELEASE=v1.4.0
+NINJA_DIR=${AOS}/externals/ninja-${NINJA_RELEASE}
NINJA=${NINJA_DIR}/ninja
# From chromium@154360:trunk/src/DEPS.
-GYP_REVISION=1488
+GYP_REVISION=1738
GYP_DIR=${AOS}/externals/gyp-${GYP_REVISION}
GYP=${GYP_DIR}/gyp
OUTDIR=${AOS}/../out_${OUT_NAME}
BUILD_NINJA=${OUTDIR}/Default/build.ninja
-[ -d ${NINJA_DIR} ] || git clone --branch release https://github.com/martine/ninja.git ${NINJA_DIR}
+[ -d ${NINJA_DIR} ] || git clone --branch ${NINJA_RELEASE} https://github.com/martine/ninja.git ${NINJA_DIR}
[ -x ${NINJA} ] || ${NINJA_DIR}/bootstrap.py
[ -d ${GYP_DIR} ] || ( svn co http://gyp.googlecode.com/svn/trunk -r ${GYP_REVISION} ${GYP_DIR} && patch -p1 -d ${GYP_DIR} < ${AOS}/externals/gyp.patch )
${AOS}/build/download_externals.sh
@@ -57,19 +63,20 @@
fi
if [ "${ACTION}" == "clean" ]; then
- rm -r ${OUTDIR}
+ rm -r ${OUTDIR} || true
else
if [ "${ACTION}" != "deploy" -a "${ACTION}" != "tests" -a "${ACTION}" != "redeploy" ]; then
- GYP_ACTION=${ACTION}
+ NINJA_ACTION=${ACTION}
else
- GYP_ACTION=
+ NINJA_ACTION=
fi
- ${NINJA} -C ${OUTDIR}/Default ${GYP_ACTION}
+ ${NINJA} -C ${OUTDIR}/Default ${NINJA_ACTION} "$@"
if [[ ${ACTION} == deploy || ${ACTION} == redeploy ]]; then
[ ${PLATFORM} == atom ] && \
rsync --progress -c -r \
- ${OUTDIR}/Default/outputs/* \
- driver@`${AOS}/build/get_ip fitpc`:/home/driver/robot_code/bin
+ ${OUTDIR}/Default/outputs/* \
+ driver@`${AOS}/build/get_ip fitpc`:/home/driver/robot_code/bin
+ ssh driver@`${AOS}/build/get_ip fitpc` "sync; sync; sync"
[ ${PLATFORM} == crio ] && \
ncftpput `${AOS}/build/get_ip robot` / \
${OUTDIR}/Default/lib/FRC_UserProgram.out
diff --git a/aos/build/crio_cc b/aos/build/crio_cc
new file mode 100755
index 0000000..442d3fe
--- /dev/null
+++ b/aos/build/crio_cc
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+# This is a helper script that gets called as a replacement for gcc. It just
+# passes all arguments on unless it is being called as a shared linker.
+
+[ $1 != '-shared' ] && exec powerpc-wrs-vxworks-gcc "$@"
+exec $(dirname $0)/crio_link_out "$@"
diff --git a/aos/build/crio_cxx b/aos/build/crio_cxx
new file mode 100755
index 0000000..ea68e58
--- /dev/null
+++ b/aos/build/crio_cxx
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+# This is a helper script that gets called as a replacement for g++. It just
+# passes all arguments on unless it is being called as a shared linker.
+
+[ $1 != '-shared' ] && exec powerpc-wrs-vxworks-g++ "$@"
+exec $(dirname $0)/crio_link_out "$@"
diff --git a/aos/build/crio_link_out b/aos/build/crio_link_out
index e5a66e7..0630341 100755
--- a/aos/build/crio_link_out
+++ b/aos/build/crio_link_out
@@ -1,7 +1,11 @@
-#!/bin/bash -e
+#!/bin/bash
+#set -x
-# This is a helper script that compiles .out files for the cRIO. It is designed
-# to be called as a replacement for g++ being used as a linker.
+set -e
+
+# This is a helper script that compiles .out files for the cRIO. It gets called
+# by the gcc and g++ wrapper scripts if they detect that the tool is being used
+# as a shared linker.
# All the flags except -shared.
INPUTS_FLAGS=`echo "$@" | sed 's/-shared//g'`
@@ -9,7 +13,6 @@
OUTPUT=`echo ${INPUTS_FLAGS} | awk \
'BEGIN { RS=" " }; output { print ; output = 0 }; /-o/ { output = 1 }'`
# All arguments that don't start with a - and aren't ${OUTPUT}.
-#INPUTS=`echo ${INPUTS_FLAGS} | sed "s:-[^ ]*::g; s:${OUTPUT}::g;"`
INPUTS=`echo ${INPUTS_FLAGS} | awk \
'BEGIN { RS=" " }; /-Wl,--no-whole-archive/ { output = 0 }; \
output { print }; \
@@ -17,7 +20,8 @@
TEMPDIR=`dirname ${OUTPUT}`
AOS=`dirname $0`/..
powerpc-wrs-vxworks-nm ${INPUTS} | \
- tclsh ${WIND_BASE}/host/resource/hutils/tcl/munch.tcl -c ppc > ${TEMPDIR}/ctdt.c
+ tclsh ${WIND_BASE}/host/resource/hutils/tcl/munch.tcl -c ppc > \
+ ${TEMPDIR}/ctdt.c
powerpc-wrs-vxworks-gcc -I${AOS}/.. -c ${TEMPDIR}/ctdt.c -o ${TEMPDIR}/ctdt.o
powerpc-wrs-vxworks-g++ ${INPUTS_FLAGS} ${TEMPDIR}/ctdt.o
ln -f ${OUTPUT} `echo ${OUTPUT} | sed 's/lib\([A-Za-z0-9_]*\)\.so$/\1.out/'`
diff --git a/aos/build/queues/output/message_dec.rb b/aos/build/queues/output/message_dec.rb
index 777e86b..3b89149 100644
--- a/aos/build/queues/output/message_dec.rb
+++ b/aos/build/queues/output/message_dec.rb
@@ -161,13 +161,13 @@
cons_ifdef_statement = CPP::PreprocessorIf.new(cons, unsafe_cons)
cons_ifdef_statement.name = "!defined(__VXWORKS__) && !defined(__TEST_VXWORKS__)"
template.add_member(:private,cons_ifdef_statement)
- cons.args << "aos_queue *queue"
+ cons.args << "RawQueue *queue"
cons.args << "#{t} *msg"
unsafe_cons.args << "#{t} *msg"
cons.add_cons("msg_ptr_","queue","msg")
unsafe_cons.add_cons("msg_ptr_","msg")
cons = safetemplate.add_member(:private,CPP::Constructor.new(safetemplate))
- cons.args << "aos_queue *queue"
+ cons.args << "RawQueue *queue"
cons.add_cons("msg_ptr_","queue")
safetemplate.public
template.public
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index 442e4ac..d8c10e0 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -21,11 +21,14 @@
'queue_testutils.cc',
],
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
'<(AOS)/build/aos.gyp:logging',
'once',
'<(EXTERNALS):gtest',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:shared_mem',
],
+ 'export_dependent_settings': [
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:shared_mem',
+ ],
},
{
'target_name': 'time',
@@ -54,10 +57,10 @@
},
{
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
'export_dependent_settings': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
}]
],
@@ -134,12 +137,14 @@
'<(AOS)/build/aos.gyp:logging',
'timing',
'time',
+ 'control_loop_queues',
],
'export_dependent_settings': [
'<(AOS)/common/messages/messages.gyp:aos_queues',
'<(AOS)/build/aos.gyp:logging',
'timing',
'time',
+ 'control_loop_queues',
],
},
{
@@ -216,6 +221,24 @@
],
},
{
+ 'target_name': 'condition',
+ 'type': 'static_library',
+ 'sources': [
+ '<(AOS)/atom_code/ipc_lib/condition.cc',
+ ],
+ 'dependencies': [
+ 'mutex',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:aos_sync',
+ # TODO(aschuh): Fix this dependency loop by
+ # providing a logging interface.
+ # '<(AOS)/build/aos.gyp:logging',
+ ],
+ 'export_dependent_settings': [
+ 'mutex',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:aos_sync',
+ ],
+ },
+ {
'target_name': 'mutex',
'type': 'static_library',
'conditions': [
@@ -228,10 +251,10 @@
'<(AOS)/atom_code/ipc_lib/mutex.cpp',
],
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:aos_sync',
],
'export_dependent_settings': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:aos_sync',
],
}],
],
@@ -254,6 +277,23 @@
],
},
{
+ 'target_name': 'condition_test',
+ 'type': 'executable',
+ 'sources': [
+ 'condition_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ 'condition',
+ '<(AOS)/common/util/util.gyp:thread',
+ 'time',
+ 'mutex',
+ '<(AOS)/build/aos.gyp:logging',
+ 'queue_testutils',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:core_lib',
+ ],
+ },
+ {
'target_name': 'die_test',
'type': 'executable',
'sources': [
diff --git a/aos/common/condition.h b/aos/common/condition.h
new file mode 100644
index 0000000..c407070
--- /dev/null
+++ b/aos/common/condition.h
@@ -0,0 +1,79 @@
+#ifndef AOS_COMMON_CONDITION_H_
+#define AOS_COMMON_CONDITION_H_
+
+#include "aos/common/mutex.h"
+#include "aos/atom_code/ipc_lib/aos_sync.h"
+
+namespace aos {
+
+// A condition variable (IPC mechanism where 1 process/task can notify all
+// others that are waiting for something to happen) without the race condition
+// where a notification is sent after some process has checked if the thing has
+// happened but before it has started listening for notifications.
+//
+// This implementation will print debugging information and abort the process
+// if anything weird happens.
+//
+// A simple example of the use of a condition variable (adapted from
+// pthread_cond(3)):
+//
+// int x, y;
+// Mutex mutex;
+// Condition condition(&mutex);
+//
+// // Waiting until x is greater than y:
+// {
+// MutexLocker locker(&mutex);
+// while (!(x > y)) condition.Wait();
+// // do whatever
+// }
+//
+// // Modifying x and/or y:
+// {
+// MutexLocker locker(&mutex);
+// // modify x and y
+// if (x > y) condition.Broadcast();
+// }
+//
+// Notice the loop around the Wait(). This is very important because some other
+// process can lock the mutex and modify the shared state (possibly undoing
+// whatever the Wait()er was waiting for) in between the Broadcast()er unlocking
+// the mutex and the Wait()er(s) relocking it.
+//
+// Multiple condition variables may be associated with the same mutex but
+// exactly 1 mutex must be associated with each condition variable.
+class Condition {
+ public:
+ // m is the mutex that will be associated with this condition variable. This
+ // object will hold on to a reference to it but does not take ownership.
+ explicit Condition(Mutex *m);
+
+ // Waits for the condition variable to be signalled, atomically unlocking the
+ // mutex associated with this condition variable at the same time. The mutex
+ // associated with this condition variable must be locked when this is called
+ // and will be locked when this method returns.
+ // NOTE: The relocking of the mutex is not performed atomically with waking
+ // up.
+ void Wait();
+
+ // Signals at most 1 other process currently Wait()ing on this condition
+ // variable. Calling this does not require the mutex associated with this
+ // condition variable to be locked.
+ // One of the processes with the highest priority level will be woken.
+ void Signal();
+ // Wakes all processes that are currently Wait()ing on this condition
+ // variable. Calling this does not require the mutex associated with this
+ // condition variable to be locked.
+ void Broadcast();
+
+ // Retrieves the mutex associated with this condition variable.
+ Mutex *m() { return m_; }
+
+ private:
+ mutex impl_;
+ Mutex *m_;
+};
+
+} // namespace aos
+
+#endif // AOS_COMMON_CONDITION_H_
diff --git a/aos/common/condition_test.cc b/aos/common/condition_test.cc
new file mode 100644
index 0000000..fca2820
--- /dev/null
+++ b/aos/common/condition_test.cc
@@ -0,0 +1,308 @@
+#include "aos/common/condition.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/wait.h>
+
+#include "gtest/gtest.h"
+
+#include "aos/common/time.h"
+#include "aos/common/mutex.h"
+#include "aos/common/queue_testutils.h"
+#include "aos/common/type_traits.h"
+#include "aos/atom_code/ipc_lib/core_lib.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/macros.h"
+
+using ::aos::time::Time;
+using ::aos::common::testing::GlobalCoreInstance;
+
+namespace aos {
+namespace testing {
+
+class ConditionTest : public ::testing::Test {
+ public:
+ struct Shared {
+ Shared() : condition(&mutex) {}
+
+ Mutex mutex;
+ Condition condition;
+ };
+ static_assert(shm_ok<Shared>::value,
+ "it's going to get shared between forked processes");
+
+ ConditionTest() : shared_(static_cast<Shared *>(shm_malloc(sizeof(Shared)))) {
+ new (shared_) Shared();
+ }
+
+ GlobalCoreInstance my_core;
+
+ Shared *const shared_;
+
+ void Settle() {
+ time::SleepFor(::Time::InSeconds(0.008));
+ }
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(ConditionTest);
+};
+
+class ConditionTestProcess {
+ public:
+ enum class Action {
+ kWaitLockStart, // lock, delay, wait, unlock
+ kWait, // delay, lock, wait, unlock
+ kWaitNoUnlock, // delay, lock, wait
+ };
+
+ // This amount gets added to any passed in delay to make the test repeatable.
+ static constexpr ::Time kMinimumDelay = ::Time::InSeconds(0.015);
+ static constexpr ::Time kDefaultTimeout = ::Time::InSeconds(0.09);
+
+ // delay is how long to wait before doing action to condition.
+ // timeout is how long to wait after delay before deciding that it's hung.
+ ConditionTestProcess(const ::Time &delay, Action action, Condition *condition,
+ const ::Time &timeout = kDefaultTimeout)
+ : delay_(kMinimumDelay + delay), action_(action), condition_(condition),
+ timeout_(delay_ + timeout), child_(-1),
+ shared_(static_cast<Shared *>(shm_malloc(sizeof(Shared)))) {
+ new (shared_) Shared();
+ }
+ ~ConditionTestProcess() {
+ assert(child_ == -1);
+ }
+
+ void Start() {
+ ASSERT_FALSE(shared_->started);
+
+ child_ = fork();
+ if (child_ == 0) { // in child
+ ::aos::common::testing::PreventExit();
+ Run();
+ exit(EXIT_SUCCESS);
+ } else { // in parent
+ assert(child_ != -1);
+
+ shared_->ready.Lock();
+
+ shared_->started = true;
+ }
+ }
+
+ bool IsFinished() {
+ return shared_->finished;
+ }
+
+ ::testing::AssertionResult Hung() {
+ if (!shared_->started) {
+ ADD_FAILURE();
+ return ::testing::AssertionFailure() << "not started yet";
+ }
+ if (shared_->finished) {
+ Join();
+ return ::testing::AssertionFailure() << "already returned";
+ }
+ if (shared_->delayed) {
+ if (shared_->start_time > ::Time::Now() + timeout_) {
+ Kill();
+ return ::testing::AssertionSuccess() << "already been too long";
+ }
+ } else {
+ shared_->done_delaying.Lock();
+ }
+ time::SleepFor(::Time::InSeconds(0.01));
+ if (!shared_->finished) time::SleepUntil(shared_->start_time + timeout_);
+ if (shared_->finished) {
+ Join();
+ return ::testing::AssertionFailure() << "completed within timeout";
+ } else {
+ Kill();
+ return ::testing::AssertionSuccess() << "took too long";
+ }
+ }
+ ::testing::AssertionResult Test() {
+ Start();
+ return Hung();
+ }
+
+ private:
+ struct Shared {
+ Shared()
+ : started(false), delayed(false), start_time(0, 0), finished(false) {
+ done_delaying.Lock();
+ ready.Lock();
+ }
+
+ volatile bool started;
+ volatile bool delayed;
+ Mutex done_delaying;
+ ::Time start_time;
+ volatile bool finished;
+ Mutex ready;
+ };
+ static_assert(shm_ok<Shared>::value,
+ "it's going to get shared between forked processes");
+
+ void Run() {
+ if (action_ == Action::kWaitLockStart) {
+ shared_->ready.Unlock();
+ condition_->m()->Lock();
+ }
+ time::SleepFor(delay_);
+ shared_->start_time = ::Time::Now();
+ shared_->delayed = true;
+ shared_->done_delaying.Unlock();
+ if (action_ != Action::kWaitLockStart) {
+ shared_->ready.Unlock();
+ condition_->m()->Lock();
+ }
+ condition_->Wait();
+ shared_->finished = true;
+ if (action_ != Action::kWaitNoUnlock) {
+ condition_->m()->Unlock();
+ }
+ }
+
+ void Join() {
+ assert(child_ != -1);
+ int status;
+ do {
+ assert(waitpid(child_, &status, 0) == child_);
+ } while (!(WIFEXITED(status) || WIFSIGNALED(status)));
+ child_ = -1;
+ }
+ void Kill() {
+ assert(child_ != -1);
+ assert(kill(child_, SIGTERM) == 0);
+ Join();
+ }
+
+ const ::Time delay_;
+ const Action action_;
+ Condition *const condition_;
+ const ::Time timeout_;
+
+ pid_t child_;
+
+ Shared *const shared_;
+
+ DISALLOW_COPY_AND_ASSIGN(ConditionTestProcess);
+};
+constexpr ::Time ConditionTestProcess::kMinimumDelay;
+constexpr ::Time ConditionTestProcess::kDefaultTimeout;
+
+// Makes sure that the testing framework and everything work for a really simple
+// Wait() and then Signal().
+TEST_F(ConditionTest, Basic) {
+ ConditionTestProcess child(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ child.Start();
+ Settle();
+ EXPECT_FALSE(child.IsFinished());
+ shared_->condition.Signal();
+ EXPECT_FALSE(child.Hung());
+}
+
+// Makes sure that the worker child locks before it tries to Wait() etc.
+TEST_F(ConditionTest, Locking) {
+ ConditionTestProcess child(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ shared_->mutex.Lock();
+ child.Start();
+ Settle();
+ // This Signal() shouldn't do anything because the child should still be
+ // waiting to lock the mutex.
+ shared_->condition.Signal();
+ Settle();
+ shared_->mutex.Unlock();
+ EXPECT_TRUE(child.Hung());
+}
+
+// Tests that the work child only catches a Signal() after the mutex gets
+// unlocked.
+TEST_F(ConditionTest, LockFirst) {
+ ConditionTestProcess child(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ shared_->mutex.Lock();
+ child.Start();
+ Settle();
+ shared_->condition.Signal();
+ Settle();
+ EXPECT_FALSE(child.IsFinished());
+ shared_->mutex.Unlock();
+ Settle();
+ EXPECT_FALSE(child.IsFinished());
+ shared_->condition.Signal();
+ EXPECT_FALSE(child.Hung());
+}
+
+// Tests that the mutex gets relocked after Wait() returns.
+TEST_F(ConditionTest, Relocking) {
+ ConditionTestProcess child(::Time(0, 0),
+ ConditionTestProcess::Action::kWaitNoUnlock,
+ &shared_->condition);
+ child.Start();
+ Settle();
+ shared_->condition.Signal();
+ EXPECT_FALSE(child.Hung());
+ EXPECT_FALSE(shared_->mutex.TryLock());
+}
+
+// Tests that Signal() stops exactly 1 Wait()er.
+TEST_F(ConditionTest, SignalOne) {
+ ConditionTestProcess child1(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ ConditionTestProcess child2(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ ConditionTestProcess child3(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ auto number_finished = [&]() { return (child1.IsFinished() ? 1 : 0) +
+ (child2.IsFinished() ? 1 : 0) + (child3.IsFinished() ? 1 : 0); };
+ child1.Start();
+ child2.Start();
+ child3.Start();
+ Settle();
+ EXPECT_EQ(0, number_finished());
+ shared_->condition.Signal();
+ Settle();
+ EXPECT_EQ(1, number_finished());
+ shared_->condition.Signal();
+ Settle();
+ EXPECT_EQ(2, number_finished());
+ shared_->condition.Signal();
+ Settle();
+ EXPECT_EQ(3, number_finished());
+ EXPECT_FALSE(child1.Hung());
+ EXPECT_FALSE(child2.Hung());
+ EXPECT_FALSE(child3.Hung());
+}
+
+// Tests that Brodcast() wakes multiple Wait()ers.
+TEST_F(ConditionTest, Broadcast) {
+ ConditionTestProcess child1(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ ConditionTestProcess child2(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ ConditionTestProcess child3(::Time(0, 0),
+ ConditionTestProcess::Action::kWait,
+ &shared_->condition);
+ child1.Start();
+ child2.Start();
+ child3.Start();
+ Settle();
+ shared_->condition.Broadcast();
+ EXPECT_FALSE(child1.Hung());
+ EXPECT_FALSE(child2.Hung());
+ EXPECT_FALSE(child3.Hung());
+}
+
+} // namespace testing
+} // namespace aos
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index a5666a9..69c3121 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -9,16 +9,16 @@
// TODO(aschuh): Tests.
-template <class T, bool has_position>
-void ControlLoop<T, has_position>::ZeroOutputs() {
+template <class T, bool has_position, bool fail_no_position>
+void ControlLoop<T, has_position, fail_no_position>::ZeroOutputs() {
aos::ScopedMessagePtr<OutputType> output =
control_loop_->output.MakeMessage();
Zero(output.get());
output.Send();
}
-template <class T, bool has_position>
-void ControlLoop<T, has_position>::Iterate() {
+template <class T, bool has_position, bool fail_no_position>
+void ControlLoop<T, has_position, fail_no_position>::Iterate() {
// Temporary storage for printing out inputs and outputs.
char state[1024];
@@ -60,8 +60,10 @@
}
} else {
LOG(ERROR, "Never had a position.\n");
- ZeroOutputs();
- return;
+ if (fail_no_position) {
+ ZeroOutputs();
+ return;
+ }
}
}
if (position) {
@@ -112,8 +114,8 @@
status.Send();
}
-template <class T, bool has_position>
-void ControlLoop<T, has_position>::Run() {
+template <class T, bool has_position, bool fail_no_position>
+void ControlLoop<T, has_position, fail_no_position>::Run() {
while (true) {
time::SleepUntil(NextLoopTime());
Iterate();
diff --git a/aos/common/control_loop/ControlLoop.cc b/aos/common/control_loop/ControlLoop.cc
index f5253d4..ea62d85 100644
--- a/aos/common/control_loop/ControlLoop.cc
+++ b/aos/common/control_loop/ControlLoop.cc
@@ -4,8 +4,8 @@
namespace control_loops {
time::Time NextLoopTime(time::Time start) {
- return (start / kLoopFrequency.ToNSec()) *
- kLoopFrequency.ToNSec() +
+ return (start / static_cast<int32_t>(kLoopFrequency.ToNSec())) *
+ static_cast<int32_t>(kLoopFrequency.ToNSec()) +
kLoopFrequency;
}
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index 723d766..6af7235 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -37,7 +37,7 @@
};
// Control loops run this often, "starting" at time 0.
-const time::Time kLoopFrequency = time::Time::InSeconds(0.01);
+constexpr time::Time kLoopFrequency = time::Time::InSeconds(0.01);
// Calculates the next time to run control loops after start.
time::Time NextLoopTime(time::Time start = time::Time::Now());
@@ -50,7 +50,7 @@
// If has_position is false, the control loop will always use NULL as the
// position and not check the queue. This is used for "loops" that control
// motors open loop.
-template <class T, bool has_position = true>
+template <class T, bool has_position = true, bool fail_no_position = true>
class ControlLoop : public SerializableControlLoop {
public:
// Maximum age of position packets before the loop will be disabled due to
diff --git a/aos/common/control_loop/Timing.cpp b/aos/common/control_loop/Timing.cpp
index 3a58036..8f42623 100644
--- a/aos/common/control_loop/Timing.cpp
+++ b/aos/common/control_loop/Timing.cpp
@@ -11,8 +11,8 @@
void PhasedLoopXMS(int ms, int offset) {
// TODO(brians): Tests!
const Time frequency = Time::InMS(ms);
- SleepUntil((Time::Now() / frequency.ToNSec()) *
- frequency.ToNSec() +
+ SleepUntil((Time::Now() / static_cast<int32_t>(frequency.ToNSec())) *
+ static_cast<int32_t>(frequency.ToNSec()) +
frequency + Time::InUS(offset));
}
diff --git a/aos/common/logging/logging.h b/aos/common/logging/logging.h
index a5f07bb..3f32be3 100644
--- a/aos/common/logging/logging.h
+++ b/aos/common/logging/logging.h
@@ -6,6 +6,7 @@
#include <stdio.h>
#include <stdint.h>
+#include <stdlib.h>
#ifdef __VXWORKS__
// Because the vxworks system headers miss the noreturn...
@@ -112,12 +113,112 @@
format, ##args); \
} while (0)
-// TODO(brians) add CHECK macros like glog
-// (<http://google-glog.googlecode.com/svn/trunk/doc/glog.html>)
-// and replace assert with one
-
#ifdef __cplusplus
}
#endif
+#ifdef __cplusplus
+
+namespace aos {
+
+// CHECK* macros, similar to glog
+// (<http://google-glog.googlecode.com/svn/trunk/doc/glog.html>)'s, except they
+// don't support streaming in extra text. Some of the implementation is borrowed
+// from there too.
+// They all LOG(FATAL) with a helpful message when the check fails.
+// TODO(brians): Replace assert with CHECK
+// Portions copyright (c) 1999, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above
+// copyright notice, this list of conditions and the following disclaimer
+// in the documentation and/or other materials provided with the
+// distribution.
+// * Neither the name of Google Inc. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// CHECK dies with a fatal error if condition is not true. It is *not*
+// controlled by NDEBUG, so the check will be executed regardless of
+// compilation mode. Therefore, it is safe to do things like:
+// CHECK(fp->Write(x) == 4)
+#define CHECK(condition) \
+ if (__builtin_expect(!(condition), 0)) { \
+ LOG(FATAL, "CHECK(" #condition ") failed\n"); \
+ }
+
+// Helper functions for CHECK_OP macro.
+// The (int, int) specialization works around the issue that the compiler
+// will not instantiate the template version of the function on values of
+// unnamed enum type.
+#define DEFINE_CHECK_OP_IMPL(name, op) \
+ template <typename T1, typename T2> \
+ inline void LogImpl##name(const T1& v1, const T2& v2, \
+ const char* exprtext) { \
+ if (!__builtin_expect(v1 op v2, 1)) { \
+ LOG(FATAL, "CHECK(%s) failed\n", exprtext); \
+ } \
+ } \
+ inline void LogImpl##name(int v1, int v2, const char* exprtext) { \
+ ::aos::LogImpl##name<int, int>(v1, v2, exprtext); \
+ }
+
+// We use the full name Check_EQ, Check_NE, etc. in case the file including
+// base/logging.h provides its own #defines for the simpler names EQ, NE, etc.
+// This happens if, for example, those are used as token names in a
+// yacc grammar.
+DEFINE_CHECK_OP_IMPL(Check_EQ, ==) // Compilation error with CHECK_EQ(NULL, x)?
+DEFINE_CHECK_OP_IMPL(Check_NE, !=) // Use CHECK(x == NULL) instead.
+DEFINE_CHECK_OP_IMPL(Check_LE, <=)
+DEFINE_CHECK_OP_IMPL(Check_LT, < )
+DEFINE_CHECK_OP_IMPL(Check_GE, >=)
+DEFINE_CHECK_OP_IMPL(Check_GT, > )
+
+#define CHECK_OP(name, op, val1, val2) \
+ ::aos::LogImplCheck##name(val1, val2, \
+ STRINGIFY(val1) STRINGIFY(op) STRINGIFY(val2))
+
+#define CHECK_EQ(val1, val2) CHECK_OP(_EQ, ==, val1, val2)
+#define CHECK_NE(val1, val2) CHECK_OP(_NE, !=, val1, val2)
+#define CHECK_LE(val1, val2) CHECK_OP(_LE, <=, val1, val2)
+#define CHECK_LT(val1, val2) CHECK_OP(_LT, < , val1, val2)
+#define CHECK_GE(val1, val2) CHECK_OP(_GE, >=, val1, val2)
+#define CHECK_GT(val1, val2) CHECK_OP(_GT, > , val1, val2)
+
+// A small helper for CHECK_NOTNULL().
+template <typename T>
+inline T* CheckNotNull(const char *value_name, T *t) {
+ if (t == NULL) {
+ LOG(FATAL, "'%s' must not be NULL\n", value_name);
+ }
+ return t;
+}
+
+// Check that the input is non NULL. This very useful in constructor
+// initializer lists.
+#define CHECK_NOTNULL(val) \
+ ::aos::CheckNotNull(STRINGIFY(val), val)
+
+} // namespace aos
+
+#endif // __cplusplus
+
#endif
diff --git a/aos/common/macros.h b/aos/common/macros.h
index 88fc52e..2018b36 100644
--- a/aos/common/macros.h
+++ b/aos/common/macros.h
@@ -6,8 +6,8 @@
// A macro to disallow the copy constructor and operator= functions
// This should be used in the private: declarations for a class
#define DISALLOW_COPY_AND_ASSIGN(TypeName) \
- TypeName(const TypeName&); \
- void operator=(const TypeName&)
+ TypeName(const TypeName&) = delete; \
+ void operator=(const TypeName&) = delete
// A macro to wrap arguments to macros that contain commas.
// Useful for DISALLOW_COPY_AND_ASSIGNing templated types with multiple template
// arguments.
diff --git a/aos/common/messages/QueueHolder.h b/aos/common/messages/QueueHolder.h
index 23ddc95..8d8ba51 100644
--- a/aos/common/messages/QueueHolder.h
+++ b/aos/common/messages/QueueHolder.h
@@ -71,7 +71,7 @@
#define aos_check_rv __attribute__((warn_unused_result))
template<typename T> class QueueHolderNoBuilder {
#ifndef __VXWORKS__
- aos_queue *const queue_;
+ Queue *const queue_;
static_assert(shm_ok<T>::value, "T must be able to"
" go through shared memory and memcpy");
T t_;
@@ -80,7 +80,7 @@
#endif
public:
#ifndef __VXWORKS__
- explicit QueueHolderNoBuilder(aos_queue *queue) : queue_(queue) {}
+ explicit QueueHolderNoBuilder(Queue *queue) : queue_(queue) {}
#else
QueueHolderNoBuilder() {}
#endif
@@ -158,7 +158,7 @@
QueueBuilder<T> builder_;
public:
#ifndef __VXWORKS__
- explicit QueueHolder(aos_queue *queue) : QueueHolderNoBuilder<T>(queue),
+ explicit QueueHolder(Queue *queue) : QueueHolderNoBuilder<T>(queue),
builder_(*this) {}
#else
QueueHolder() : builder_(*this) {}
@@ -171,7 +171,6 @@
}
};
-} // namespace aos
+} // namespace aos
#endif
-
diff --git a/aos/common/messages/messages.gyp b/aos/common/messages/messages.gyp
index 3b8d389..b76dbba 100644
--- a/aos/common/messages/messages.gyp
+++ b/aos/common/messages/messages.gyp
@@ -19,10 +19,10 @@
'conditions': [
['OS!="crio"', {
'dependencies': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
'export_dependent_settings': [
- '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:ipc_lib',
+ '<(AOS)/atom_code/ipc_lib/ipc_lib.gyp:queue',
],
}],
],
diff --git a/aos/common/mutex.h b/aos/common/mutex.h
index 035889b..b6b277c 100644
--- a/aos/common/mutex.h
+++ b/aos/common/mutex.h
@@ -11,6 +11,8 @@
namespace aos {
+class Condition;
+
// An abstraction of a mutex that has implementations both for the
// atom and for the cRIO.
// If there are multiple tasks or processes contending for the mutex,
@@ -42,6 +44,9 @@
typedef mutex ImplementationType;
#endif
ImplementationType impl_;
+
+ friend class Condition; // for access to impl_
+
#ifdef __VXWORKS__
DISALLOW_COPY_AND_ASSIGN(Mutex);
#endif
@@ -64,6 +69,20 @@
Mutex *mutex_;
DISALLOW_COPY_AND_ASSIGN(MutexLocker);
};
+// The inverse of MutexLocker.
+class MutexUnlocker {
+ public:
+ explicit MutexUnlocker(Mutex *mutex) : mutex_(mutex) {
+ mutex_->Unlock();
+ }
+ ~MutexUnlocker() {
+ mutex_->Lock();
+ }
+
+ private:
+ Mutex *mutex_;
+ DISALLOW_COPY_AND_ASSIGN(MutexUnlocker);
+};
} // namespace aos
diff --git a/aos/common/mutex_test.cpp b/aos/common/mutex_test.cpp
index a5d5e86..652cd9e 100644
--- a/aos/common/mutex_test.cpp
+++ b/aos/common/mutex_test.cpp
@@ -1,7 +1,16 @@
#include "aos/common/mutex.h"
+#include <sched.h>
+#include <math.h>
+#include <pthread.h>
+#ifdef __VXWORKS__
+#include <taskLib.h>
+#endif
+
#include "gtest/gtest.h"
+#include "aos/atom_code/ipc_lib/aos_sync.h"
+
namespace aos {
namespace testing {
@@ -51,5 +60,17 @@
EXPECT_TRUE(test_mutex.TryLock());
}
+TEST_F(MutexTest, MutexUnlocker) {
+ test_mutex.Lock();
+ {
+ aos::MutexUnlocker unlocker(&test_mutex);
+ // If this fails, then something weird is going on and the next line might
+ // hang, so fail immediately.
+ ASSERT_TRUE(test_mutex.TryLock());
+ test_mutex.Unlock();
+ }
+ EXPECT_FALSE(test_mutex.TryLock());
+}
+
} // namespace testing
} // namespace aos
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 612429a..68cb338 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -138,7 +138,7 @@
#ifndef USE_UNSAFE
// Only Queue should be able to build a queue.
- ScopedMessagePtr(aos_queue *queue, T *msg)
+ ScopedMessagePtr(RawQueue *queue, T *msg)
: queue_(queue), msg_(msg) {}
#else
ScopedMessagePtr(T *msg)
@@ -152,10 +152,10 @@
#ifndef USE_UNSAFE
// Sets the queue that owns this message.
- void set_queue(aos_queue *queue) { queue_ = queue; }
+ void set_queue(RawQueue *queue) { queue_ = queue; }
// The queue that the message is a part of.
- aos_queue *queue_;
+ RawQueue *queue_;
#endif // USE_UNSAFE
// The message or NULL.
T *msg_;
@@ -281,7 +281,7 @@
#else
T *MakeRawMessage();
// Pointer to the queue that this object fetches from.
- aos_queue *queue_;
+ RawQueue *queue_;
#endif
// Scoped pointer holding the latest message or NULL.
ScopedMessagePtr<const T> queue_msg_;
diff --git a/aos/common/queue_test.cc b/aos/common/queue_test.cc
index 65a1c25..32d1d23 100644
--- a/aos/common/queue_test.cc
+++ b/aos/common/queue_test.cc
@@ -48,7 +48,7 @@
usleep(50000);
my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
t.Join();
- EXPECT_EQ(true, t.threaded_test_queue.IsNewerThanMS(20));
+ EXPECT_LE(t.threaded_test_queue.Age(), time::Time::InMS(55));
}
// Tests that we can send a message with the message pointer and get it back.
diff --git a/aos/common/queue_testutils.cc b/aos/common/queue_testutils.cc
index 1d47e62..8d195c5 100644
--- a/aos/common/queue_testutils.cc
+++ b/aos/common/queue_testutils.cc
@@ -1,6 +1,10 @@
#include "aos/common/queue_testutils.h"
#include <string.h>
+#include <sys/mman.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <assert.h>
#include "gtest/gtest.h"
@@ -110,15 +114,23 @@
Once<void> enable_test_logging_once(DoEnableTestLogging);
+const size_t kCoreSize = 0x100000;
+
+void TerminateExitHandler() {
+ _exit(EXIT_SUCCESS);
+}
+
} // namespace
GlobalCoreInstance::GlobalCoreInstance() {
- const size_t kCoreSize = 0x100000;
global_core = &global_core_data_;
- global_core->owner = 1;
- void *memory = malloc(kCoreSize);
- assert(memory != NULL);
- memset(memory, 0, kCoreSize);
+ global_core->owner = true;
+ // Use mmap(2) manually instead of through malloc(3) so that we can pass
+ // MAP_SHARED which allows forked processes to communicate using the
+ // "shared" memory.
+ void *memory = mmap(NULL, kCoreSize, PROT_READ | PROT_WRITE,
+ MAP_SHARED | MAP_ANONYMOUS, -1, 0);
+ assert(memory != MAP_FAILED);
assert(aos_core_use_address_as_shared_mem(memory, kCoreSize) == 0);
@@ -126,7 +138,7 @@
}
GlobalCoreInstance::~GlobalCoreInstance() {
- free(global_core->mem_struct);
+ assert(munmap(global_core->mem_struct, kCoreSize) == 0);
global_core = NULL;
}
@@ -134,6 +146,10 @@
enable_test_logging_once.Get();
}
+void PreventExit() {
+ assert(atexit(TerminateExitHandler) == 0);
+}
+
} // namespace testing
} // namespace common
} // namespace aos
diff --git a/aos/common/queue_testutils.h b/aos/common/queue_testutils.h
index aabdd2d..2d26262 100644
--- a/aos/common/queue_testutils.h
+++ b/aos/common/queue_testutils.h
@@ -1,11 +1,20 @@
-#include "aos/common/queue.h"
+#ifndef AOS_COMMON_QUEUE_TESTUTILS_H_
+#define AOS_COMMON_QUEUE_TESTUTILS_H_
+
+#include "aos/atom_code/ipc_lib/shared_mem.h"
+
+// This file has some general helper functions for dealing with testing things
+// that use shared memory etc.
namespace aos {
namespace common {
namespace testing {
+// Manages creating and cleaning up "shared memory" which works within this
+// process and any that it fork(2)s.
class GlobalCoreInstance {
public:
+ // Calls EnableTestLogging().
GlobalCoreInstance();
~GlobalCoreInstance();
@@ -20,6 +29,14 @@
// initialized), however it can be called more than that.
void EnableTestLogging();
+// Registers an exit handler (using atexit(3)) which will call _exit(2).
+// Intended to be called in a freshly fork(2)ed process where it will run before
+// any other exit handlers that were already registered and prevent them from
+// being run.
+void PreventExit();
+
} // namespace testing
} // namespace common
} // namespace aos
+
+#endif // AOS_COMMON_QUEUE_TESTUTILS_H_
diff --git a/aos/common/sensors/sensor_packer.h b/aos/common/sensors/sensor_packer.h
deleted file mode 100644
index cfcf045..0000000
--- a/aos/common/sensors/sensor_packer.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef AOS_COMMON_SENSORS_SENSOR_PACKER_H_
-#define AOS_COMMON_SENSORS_SENSOR_PACKER_H_
-
-namespace aos {
-namespace sensors {
-
-// An interface that handles reading input data and putting it into the sensor
-// values struct.
-// See sensors.h for an overview of where this fits in.
-template<class Values>
-class SensorPackerInterface {
- public:
- virtual ~SensorPackerInterface() {}
-
- // Reads the inputs (from WPILib etc) and writes the data into *values.
- virtual void PackInto(Values *values) = 0;
-};
-
-} // namespace sensors
-} // namespace aos
-
-#endif // AOS_COMMON_SENSORS_SENSOR_PACKER_H_
diff --git a/aos/common/sensors/sensor_receiver-tmpl.h b/aos/common/sensors/sensor_receiver-tmpl.h
deleted file mode 100644
index 8fb6cb4..0000000
--- a/aos/common/sensors/sensor_receiver-tmpl.h
+++ /dev/null
@@ -1,264 +0,0 @@
-#include "aos/common/inttypes.h"
-#include "aos/common/network_port.h"
-
-namespace aos {
-namespace sensors {
-
-template<class Values>
-const time::Time SensorReceiver<Values>::kJitterDelay =
- time::Time::InSeconds(0.003);
-// Not a multiple of kSensorSendFrequency to unwedge ourself if we hit some bug
-// where it needs to get off of that frequency to work.
-template<class Values>
-const time::Time SensorReceiver<Values>::kGiveupTime =
- time::Time::InSeconds(0.1555);
-
-template<class Values>
-SensorReceiver<Values>::SensorReceiver(
- SensorUnpackerInterface<Values> *unpacker)
- : unpacker_(unpacker),
- start_time_(0, 0),
- last_good_time_(0, 0) {
- Unsynchronize();
-}
-
-template<class Values>
-void SensorReceiver<Values>::RunIteration() {
- if (synchronized_) {
- if (ReceiveData()) {
- LOG(DEBUG, "receive said to try a reset\n");
- Unsynchronize();
- } else {
- // TODO(brians): resync on crio reboots (and update the times...)
- if (GoodPacket()) {
- unpacker_->UnpackFrom(&data_.values);
- last_good_time_ = time::Time::Now();
- } else {
- if ((time::Time::Now() - last_good_time_) > kGiveupTime) {
- LOG(INFO, "resetting because didn't get a good one in too long\n");
- Unsynchronize();
- } else {
- // We got a packet, but it wasn't an interesting one.
- }
- }
- }
- } else {
- LOG(INFO, "resetting to try receiving data\n");
- Reset();
- if (Synchronize()) {
- LOG(INFO, "synchronized successfully\n");
- synchronized_ = true;
- before_better_cycles_ = after_better_cycles_ = 0;
- last_good_time_ = time::Time::Now();
- } else {
- LOG(INFO, "synchronization failed\n");
- }
- }
-}
-
-template<class Values>
-bool SensorReceiver<Values>::GoodPacket() {
- bool good;
- // If it's a multiple of kSensorSendFrequency from start_count_.
- if (((data_.count - start_count_) % kSendsPerCycle) == 0) {
- if (((data_.count - start_count_) / kSendsPerCycle) >=
- ((NextLoopTime() - start_time_).ToNSec() / kLoopFrequency.ToNSec())) {
- good = true;
-#if 0
- if (((data_.count - start_count_) / kSendsPerCycle % 20) == 0) {
- LOG(DEBUG, "dropping one for fun\n");
- good = false;
- }
-#endif
- } else {
- LOG(INFO,
- "packet %" PRId32 " late. is packet #%d, wanted #%" PRId64 " now\n",
- data_.count, (data_.count - start_count_) / kSendsPerCycle,
- (NextLoopTime() - start_time_).ToNSec() / kLoopFrequency.ToNSec());
- good = false;
- }
- } else {
- good = false;
- }
-
- static time::Time last_time(0, 0);
- time::Time now = time::Time::Now();
- time::Time next_goal_time = NextLoopTime() - kJitterDelay;
- // If this is the packet after the right one.
- if (((data_.count - start_count_ - 1) % kSendsPerCycle) == 0) {
- // If this one is much closer than the last one (aka the one that we used).
- if ((now - next_goal_time).abs() * 11 / 10 <
- (last_time - next_goal_time).abs()) {
- LOG(DEBUG, "next one better than one being used %d\n",
- after_better_cycles_);
- if (after_better_cycles_ > kBadCyclesToSwitch) {
- LOG(INFO, "switching to the packet after\n");
- ++start_count_;
- before_better_cycles_ = after_better_cycles_ = 0;
- } else {
- ++after_better_cycles_;
- }
- } else {
- after_better_cycles_ = 0;
- }
- }
- // If this is the right packet.
- if (((data_.count - start_count_) % kSendsPerCycle) == 0) {
- // If the last one was closer than this one (aka the one that we used).
- if ((last_time - next_goal_time).abs() * 11 / 10 <
- (now - next_goal_time).abs()) {
- LOG(DEBUG, "previous better than one being used %d\n",
- before_better_cycles_);
- if (before_better_cycles_ > kBadCyclesToSwitch) {
- LOG(INFO, "switching to the packet before\n");
- --start_count_;
- before_better_cycles_ = after_better_cycles_ = 0;
- } else {
- ++before_better_cycles_;
- }
- } else {
- before_better_cycles_ = 0;
- }
- }
- last_time = now;
-
- return good;
-}
-
-// Looks for when the timestamps transition from before where we want to after
-// and then picks whichever one was closer. After that, reads kTestCycles and
-// makes sure that at most 1 is bad.
-template<class Values>
-bool SensorReceiver<Values>::Synchronize() {
- time::Time old_received_time(0, 0);
- const time::Time start_time = time::Time::Now();
- // When we want to send out the next set of values.
- time::Time goal_time = NextLoopTime(start_time) - kJitterDelay;
- if (goal_time <= start_time) {
- goal_time += kLoopFrequency;
- }
- assert(goal_time > start_time);
- while (true) {
- if (ReceiveData()) return false;
- time::Time received_time = time::Time::Now();
- if (received_time >= goal_time) {
- // If this was the very first one we got, try again.
- if (old_received_time == time::Time(0, 0)){
- LOG(INFO, "first one we got was too late\n");
- return false;
- }
-
- assert(old_received_time < goal_time);
-
- // If the most recent one is closer than the last one.
- if ((received_time - goal_time).abs() <
- (old_received_time - goal_time).abs()) {
- start_count_ = data_.count;
- } else {
- start_count_ = data_.count - 1;
- }
- start_time_ = goal_time;
-
- int bad_count = 0;
- for (int i = 0; i < kTestCycles;) {
- ReceiveData();
- received_time = time::Time::Now();
- if (GoodPacket()) {
- LOG(DEBUG, "checking packet count=%" PRId32
- " received at %" PRId32 "s%" PRId32 "ns\n",
- data_.count, received_time.sec(), received_time.nsec());
- // If |the difference between the goal time for this numbered packet
- // and the time we actually got this one| is too big.
- if (((goal_time +
- kSensorSendFrequency * (data_.count - start_count_)) -
- received_time).abs() > kSensorSendFrequency) {
- LOG(INFO, "rejected time of the last good packet. "
- "got %" PRId32 "s%" PRId32 "ns."
- " wanted %" PRId32 "s%" PRId32 "ns\n",
- received_time.sec(), received_time.nsec(),
- goal_time.sec(), goal_time.nsec());
- ++bad_count;
- }
- ++i;
- }
- if (bad_count > 1) {
- LOG(WARNING, "got multiple packets with bad timestamps\n");
- return false;
- }
- }
-
- Synchronized(goal_time + kLoopFrequency * kTestCycles);
- return true;
- }
-
- old_received_time = received_time;
- }
-}
-
-template<class Values>
-bool SensorReceiver<Values>::ReceiveData() {
- int old_count = data_.count;
- DoReceiveData();
-
- if (data_.count < 0) {
- LOG(FATAL, "data count overflowed. currently %" PRId32 "\n", data_.count);
- }
- if (data_.count < old_count) {
- LOG(INFO, "count reset. was %" PRId32 ", now %" PRId32 "\n",
- old_count, data_.count);
- return true;
- }
- if (data_.count < start_count_) {
- LOG(INFO, "count reset. started at %" PRId32 ", now %" PRId32 "\n",
- start_count_, data_.count);
- }
- LOG(DEBUG, "received data count %" PRId32 "\n", data_.count);
- return false;
-}
-
-template<class Values>
-void SensorReceiver<Values>::Unsynchronize() {
- synchronized_ = false;
- before_better_cycles_ = after_better_cycles_ = 0;
-}
-
-template<class Values>
-const time::Time NetworkSensorReceiver<Values>::kWarmupTime =
- time::Time::InSeconds(0.075);
-
-template<class Values>
-NetworkSensorReceiver<Values>::NetworkSensorReceiver(
- SensorUnpackerInterface<Values> *unpacker)
- : SensorReceiver<Values>(unpacker),
- socket_(NetworkPort::kSensors) {}
-
-template<class Values>
-void NetworkSensorReceiver<Values>::Reset() {
- LOG(INFO, "beginning warm up\n");
- time::Time start = time::Time::Now();
- while ((time::Time::Now() - start) < kWarmupTime) {
- socket_.Receive(this->data(), sizeof(*this->data()));
- }
- LOG(INFO, "done warming up\n");
-}
-
-template<class Values>
-void NetworkSensorReceiver<Values>::DoReceiveData() {
- while (true) {
- if (socket_.Receive(this->data(), sizeof(*this->data())) ==
- sizeof(*this->data())) {
- this->data()->checksum = ntoh(this->data()->checksum);
- if (!this->data()->CheckChecksum()) {
- LOG(WARNING, "got a bad packet\n");
- continue;
- }
-
- this->data()->NetworkToHost();
- return;
- }
- LOG(WARNING, "received incorrect amount of data\n");
- }
-}
-
-} // namespace sensors
-} // namespace aos
diff --git a/aos/common/sensors/sensor_receiver.h b/aos/common/sensors/sensor_receiver.h
deleted file mode 100644
index 335ff08..0000000
--- a/aos/common/sensors/sensor_receiver.h
+++ /dev/null
@@ -1,120 +0,0 @@
-#ifndef AOS_COMMON_SENSORS_SENSOR_RECEIVER_H_
-#define AOS_COMMON_SENSORS_SENSOR_RECEIVER_H_
-
-#include "aos/common/sensors/sensor_unpacker.h"
-#include "aos/common/network/ReceiveSocket.h"
-#include "aos/common/sensors/sensors.h"
-#include "aos/common/time.h"
-#include "aos/common/gtest_prod.h"
-
-namespace aos {
-namespace sensors {
-namespace testing {
-
-FORWARD_DECLARE_TEST_CASE(SensorReceiverTest, Simple);
-FORWARD_DECLARE_TEST_CASE(SensorReceiverTest, BadStartup2);
-FORWARD_DECLARE_TEST_CASE(SensorReceiverTest, StartTimeAndCountMismatch);
-
-} // namespace testing
-
-// A class that handles receiving sensor values from the cRIO.
-// See sensors.h for an overview of where this fits in.
-//
-// Abstract class to make testing the complex logic for choosing which data to
-// use easier.
-template<class Values>
-class SensorReceiver {
- public:
- // Does not take ownership of unpacker.
- SensorReceiver(SensorUnpackerInterface<Values> *unpacker);
-
- void RunIteration();
-
- protected:
- SensorData<Values> *data() { return &data_; }
-
- private:
- // How long before the control loops run to aim for receiving sensor data (to
- // prevent jitter if some packets arrive up to this much later).
- static const time::Time kJitterDelay;
- // How many cycles not to send data out to make sure that we're in phase
- // (during this time, the code verifies that <= 1 cycle is not within 1
- // cycle's time of kJitterDelay).
- static const int kTestCycles = 8;
- // How many cycles that we need (consecutively) of another packet being closer
- // to the right time than the ones we're reading before we switch.
- static const int kBadCyclesToSwitch = 8;
- // If we don't get a good packet in this long, then we Synchronize() again.
- static const time::Time kGiveupTime;
-
- FRIEND_TEST_NAMESPACE(SensorReceiverTest, Simple, testing);
- FRIEND_TEST_NAMESPACE(SensorReceiverTest, BadStartup2, testing);
- FRIEND_TEST_NAMESPACE(SensorReceiverTest, StartTimeAndCountMismatch, testing);
-
- // Subclasses need to implement this to read 1 set of data (blocking until it
- // is available) into data().
- // It needs to have the correct byte order etc and not be corrupted
- // (subclasses can check the checksum if they want).
- virtual void DoReceiveData() = 0;
-
- // Optional: if subclasses can do anything to reinitialize after there are
- // problems, they should do it here.
- // This will be called right before calling DoReceiveData() the first time.
- virtual void Reset() {}
-
- // Optional: if subclasses want to be notified when this is first convinced
- // that it has a good packet, they should do whatever here.
- virtual void Synchronized(time::Time /*last_packet_ideal_time*/) {}
-
- // Returns whether the current packet looks like a good one to use.
- bool GoodPacket();
-
- // Synchronizes with incoming packets and sets start_count_ to where we
- // started reading.
- // Returns whether it succeeded in locking on.
- bool Synchronize();
- // Receives a set of values and makes sure that it's sane.
- // Returns whether to start over again with timing.
- bool ReceiveData();
- void Unsynchronize();
-
- SensorUnpackerInterface<Values> *const unpacker_;
- SensorData<Values> data_;
- // The count that we started out (all other sent packets will be multiples of
- // this).
- int32_t start_count_;
- // When start_count_ "should" have been received. Used for checking to make
- // sure that we don't send out a packet late.
- time::Time start_time_;
- bool synchronized_;
- int before_better_cycles_, after_better_cycles_;
- // The time of the last packet that we sent out.
- time::Time last_good_time_;
-
- DISALLOW_COPY_AND_ASSIGN(SensorReceiver<Values>);
-};
-
-// A SensorReceiver that receives data from a SensorBroadcaster.
-template<class Values>
-class NetworkSensorReceiver : public SensorReceiver<Values> {
- public:
- NetworkSensorReceiver(SensorUnpackerInterface<Values> *unpacker);
-
- private:
- // How long to read data as fast as possible for (to clear out buffers etc).
- static const time::Time kWarmupTime;
-
- virtual void DoReceiveData();
- virtual void Reset();
-
- ReceiveSocket socket_;
-
- DISALLOW_COPY_AND_ASSIGN(NetworkSensorReceiver<Values>);
-};
-
-} // namespace sensors
-} // namespace aos
-
-#include "aos/common/sensors/sensor_receiver-tmpl.h"
-
-#endif // AOS_COMMON_SENSORS_SENSOR_RECEIVER_H_
diff --git a/aos/common/sensors/sensor_receiver_test.cc b/aos/common/sensors/sensor_receiver_test.cc
deleted file mode 100644
index 803ec47..0000000
--- a/aos/common/sensors/sensor_receiver_test.cc
+++ /dev/null
@@ -1,158 +0,0 @@
-#include "aos/common/sensors/sensor_receiver.h"
-
-#include "gtest/gtest.h"
-
-#include "aos/common/sensors/sensors.h"
-#include "aos/common/time.h"
-#include "aos/common/queue_testutils.h"
-
-using ::aos::time::Time;
-
-namespace aos {
-namespace sensors {
-namespace testing {
-
-struct TestValues {
- int count;
- int more_data;
-};
-class TestSensorReceiver : public SensorReceiver<TestValues>,
- public SensorUnpackerInterface<TestValues> {
- public:
- TestSensorReceiver()
- : SensorReceiver<TestValues>(this),
- resets_(0),
- unpacks_(0) {
- data()->count = 0;
- }
-
- void Reset() {
- LOG(DEBUG, "reset for the %dth time\n", ++resets_);
- }
- void DoReceiveData() {
- last_received_count_ = ++data()->count;
- data()->values.count = last_received_count_;
- Time::IncrementMockTime(kSensorSendFrequency);
- data()->FillinChecksum();
- data()->HostToNetwork();
- }
-
- int resets() { return resets_; }
- int unpacks() { return unpacks_; }
- using SensorReceiver<TestValues>::data;
-
- void ResetFakeData() {
- data()->count = 0;
- }
-
- void UnpackFrom(TestValues *data) {
- // Make sure that it didn't lose one that we gave it.
- EXPECT_EQ(last_received_count_, data->count);
- ++unpacks_;
- LOG(DEBUG, "%dth unpack\n", unpacks_);
- }
-
- private:
- int resets_;
- int unpacks_;
- int last_received_count_;
-};
-
-class SensorReceiverTest : public ::testing::Test {
- protected:
- SensorReceiverTest() {
- ::aos::common::testing::EnableTestLogging();
- Time::EnableMockTime(Time(971, 254));
- }
-
- TestSensorReceiver &receiver() { return receiver_; }
-
- private:
- TestSensorReceiver receiver_;
-};
-
-TEST_F(SensorReceiverTest, Simple) {
- static const int kIterations = 53;
- for (int i = 0; i < kIterations; ++i) {
- receiver().RunIteration();
- }
- EXPECT_EQ(1, receiver().resets());
- // expected value is kIterations/kSendsPerCycle (rounded up) (the number of
- // times that it should get a good one) - 1 (to compensate for the iteration
- // when it synced itself up)
- EXPECT_EQ((kIterations + kSendsPerCycle - 1) / kSendsPerCycle - 1,
- receiver().unpacks());
-}
-
-TEST_F(SensorReceiverTest, CRIOReboot) {
- for (int i = 0; i < 50; ++i) {
- receiver().RunIteration();
- if (i == 27) {
- receiver().ResetFakeData();
- time::Time::IncrementMockTime(time::Time::InSeconds(20));
- }
- }
- EXPECT_EQ(2, receiver().resets());
- EXPECT_GE(receiver().unpacks(), 4);
-}
-
-TEST_F(SensorReceiverTest, CRIOSkew) {
- for (int i = 0; i < 505; ++i) {
- receiver().RunIteration();
- time::Time::IncrementMockTime(time::Time(0, 4000));
- }
- // TODO(brians) verify here that it actually corrects (happens twice with
- // current constants)
- EXPECT_EQ(1, receiver().resets());
- EXPECT_EQ(50, receiver().unpacks());
-}
-
-TEST_F(SensorReceiverTest, BadStartup1) {
- time::Time::SetMockTime(NextLoopTime() - Time(0, 100));
- for (int i = 0; i < 55; ++i) {
- receiver().RunIteration();
- }
- EXPECT_EQ(1, receiver().resets());
- EXPECT_EQ(5, receiver().unpacks());
-}
-
-TEST_F(SensorReceiverTest, BadStartup2) {
- time::Time::SetMockTime(NextLoopTime() -
- SensorReceiver<TestValues>::kJitterDelay -
- time::Time(0, 1));
- for (int i = 0; i < 55; ++i) {
- receiver().RunIteration();
- }
- EXPECT_EQ(2, receiver().resets());
- EXPECT_EQ(5, receiver().unpacks());
-}
-
-TEST_F(SensorReceiverTest, BadStartup3) {
- time::Time::SetMockTime(NextLoopTime() -
- time::Time::InSeconds(0.002) +
- kLoopFrequency / 20);
- for (int i = 0; i < 55; ++i) {
- receiver().RunIteration();
- }
- EXPECT_EQ(1, receiver().resets());
- EXPECT_EQ(5, receiver().unpacks());
-}
-
-// I think that it somehow got this way once and never recovered.
-// It should never get this way, but if it does, it should recover.
-TEST_F(SensorReceiverTest, StartTimeAndCountMismatch) {
- for (int i = 0; i < 1005; ++i) {
- receiver().RunIteration();
- if (i == 3) {
- receiver().start_count_ += 10;
- }
- }
- EXPECT_EQ(2, receiver().resets());
- EXPECT_GT(receiver().unpacks(), 30);
-}
-
-// TODO(brians) finish writing tests and commenting them and the code
-
-} // namespace testing
-} // namespace sensors
-} // namespace aos
diff --git a/aos/common/sensors/sensor_sink.h b/aos/common/sensors/sensor_sink.h
deleted file mode 100644
index 760f90e..0000000
--- a/aos/common/sensors/sensor_sink.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef AOS_COMMON_SENSORS_SENSOR_SINK_H_
-#define AOS_COMMON_SENSORS_SENSOR_SINK_H_
-
-#include "aos/common/sensors/sensors.h"
-
-namespace aos {
-namespace sensors {
-
-// Generic class for something that can do something with sensor data.
-template<class Values>
-class SensorSinkInterface {
- public:
- virtual ~SensorSinkInterface() {}
-
- virtual void Process(SensorData<Values> *data) = 0;
-};
-
-} // namespace sensors
-} // namespace aos
-
-#endif // AOS_COMMON_SENSORS_SENSOR_SINK_H_
diff --git a/aos/common/sensors/sensor_unpacker.h b/aos/common/sensors/sensor_unpacker.h
deleted file mode 100644
index b19e0c6..0000000
--- a/aos/common/sensors/sensor_unpacker.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef AOS_COMMON_SENSORS_SENSOR_UNPACKER_H_
-#define AOS_COMMON_SENSORS_SENSOR_UNPACKER_H_
-
-namespace aos {
-namespace sensors {
-
-// An interface that handles taking data from the sensor Values struct and
-// putting it into queues (for control loops etc).
-// See sensors.h for an overview of where this fits in.
-template<class Values>
-class SensorUnpackerInterface {
- public:
- virtual ~SensorUnpackerInterface() {}
-
- // Takes the data in *values and writes it out into queues etc.
- virtual void UnpackFrom(Values *values) = 0;
-};
-
-} // namespace sensors
-} // namespace aos
-
-#endif // AOS_COMMON_SENSORS_SENSOR_UNPACKER_H_
diff --git a/aos/common/sensors/sensors.cc b/aos/common/sensors/sensors.cc
deleted file mode 100644
index 60f411f..0000000
--- a/aos/common/sensors/sensors.cc
+++ /dev/null
@@ -1,89 +0,0 @@
-#include "aos/common/sensors/sensors.h"
-
-namespace aos {
-namespace sensors {
-
-const time::Time kSensorSendFrequency =
- ::aos::control_loops::kLoopFrequency / kSendsPerCycle;
-
-namespace {
-
-// Table grabbed from <http://gcc.gnu.org/svn/gcc/trunk/libiberty/crc32.c>.
-const uint32_t crc32_table[] = {
- 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9,
- 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005,
- 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61,
- 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd,
- 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9,
- 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75,
- 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011,
- 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd,
- 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039,
- 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5,
- 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81,
- 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d,
- 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49,
- 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95,
- 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1,
- 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d,
- 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae,
- 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072,
- 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16,
- 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca,
- 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde,
- 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02,
- 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066,
- 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
- 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e,
- 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692,
- 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6,
- 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a,
- 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e,
- 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2,
- 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686,
- 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a,
- 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637,
- 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb,
- 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f,
- 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53,
- 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47,
- 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b,
- 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff,
- 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623,
- 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7,
- 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b,
- 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f,
- 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3,
- 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7,
- 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b,
- 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f,
- 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3,
- 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640,
- 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c,
- 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8,
- 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24,
- 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30,
- 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
- 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088,
- 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654,
- 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0,
- 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c,
- 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18,
- 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4,
- 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0,
- 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c,
- 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668,
- 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
-};
-
-} // namespace
-uint32_t CalculateChecksum(char *buf, size_t size) {
- uint32_t ret = ~0;
- for (size_t i = 0; i < size; ++i) {
- ret = (ret << 8) ^ crc32_table[((ret >> 24) ^ buf[i]) & 255];
- }
- return ~ret;
-}
-
-} // namespace sensors
-} // namespace aos
diff --git a/aos/common/sensors/sensors.gyp b/aos/common/sensors/sensors.gyp
deleted file mode 100644
index b8a2092..0000000
--- a/aos/common/sensors/sensors.gyp
+++ /dev/null
@@ -1,76 +0,0 @@
-{
- 'targets': [
- {
- 'target_name': 'sensor_sink',
- 'type': 'static_library',
- 'sources': [
- ],
- 'dependencies': [
- 'sensors',
- ],
- 'export_dependent_settings': [
- 'sensors',
- ],
- },
- {
- 'target_name': 'sensors',
- 'type': 'static_library',
- 'sources': [
- 'sensors.cc'
- ],
- 'dependencies': [
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:controls',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:controls',
- ],
- },
- {
- 'target_name': 'sensors_test',
- 'type': '<(aos_target)',
- 'sources': [
- 'sensors_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'sensors',
- '<(AOS)/common/common.gyp:queue_testutils',
- ],
- },
- {
- 'target_name': 'sensor_receiver',
- 'type': 'static_library',
- 'sources': [
- #'sensor_receiver-tmpl.h'
- ],
- 'dependencies': [
- '<(AOS)/common/network/network.gyp:socket',
- 'sensors',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:gtest_prod',
- ],
- 'export_dependent_settings': [
- '<(AOS)/common/network/network.gyp:socket',
- 'sensors',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:gtest_prod',
- ],
- },
- {
- 'target_name': 'sensor_receiver_test',
- 'type': 'executable',
- 'sources': [
- 'sensor_receiver_test.cc',
- ],
- 'dependencies': [
- '<(EXTERNALS):gtest',
- 'sensor_receiver',
- '<(AOS)/common/common.gyp:time',
- 'sensors',
- '<(AOS)/common/common.gyp:queue_testutils',
- ],
- },
- ],
-}
diff --git a/aos/common/sensors/sensors.h b/aos/common/sensors/sensors.h
deleted file mode 100644
index 78fd8a9..0000000
--- a/aos/common/sensors/sensors.h
+++ /dev/null
@@ -1,83 +0,0 @@
-#ifndef AOS_COMMON_SENSORS_SENSORS_H_
-#define AOS_COMMON_SENSORS_SENSORS_H_
-
-#include "aos/common/time.h"
-#include "aos/common/byteorder.h"
-#include "aos/common/control_loop/ControlLoop.h"
-#include "aos/common/inttypes.h"
-
-namespace aos {
-// This namespace contains all of the stuff for dealing with reading sensors and
-// communicating it to everything that needs it. There are 4 main classes whose
-// instances actually process the data. They must all be registered in the
-// appropriate ::aos::crio::ControlsManager hooks.
-//
-// SensorPackers get run on the cRIO to read inputs (from WPILib or elsewhere)
-// and put the values into the Values struct (which is templated for all of the
-// classes that use it).
-// SensorUnpackers get run on both the atom and the cRIO to take the data from
-// the Values struct and put them into queues for control loops etc.
-// SensorBroadcasters (on the cRIO) send the data to a SensorReceiver (on the
-// atom) to pass to its SensorUnpacker there.
-// CRIOControlLoopRunners register with a SensorBroadcaster to get called right
-// after reading the sensor data so that they can immediately pass it so a
-// SensorUnpacker and then run their control loops.
-// The actual SensorPacker and SensorUnpacker classes have the Interface suffix
-// on them.
-namespace sensors {
-
-// How many times per ::aos::control_loops::kLoopFrequency sensor
-// values get sent out by the cRIO.
-// This must evenly divide that frequency into multiples of sysClockRateGet().
-const int kSendsPerCycle = 10;
-// ::aos::control_loops::kLoopFrequency / kSendsPerCycle for
-// convenience.
-extern const time::Time kSensorSendFrequency;
-using ::aos::control_loops::kLoopFrequency;
-using ::aos::control_loops::NextLoopTime;
-
-uint32_t CalculateChecksum(char *buf, size_t size);
-
-// This is the struct that actually gets sent over the UDP socket.
-template<class Values>
-struct SensorData {
- // All of the other 4-byte chunks in the message bitwise-exclusive-ORed
- // together. Needed because it seems like nobody else checks... (vxworks not
- // sending the UDP checksum or (not very likely) linux not checking it).
- // TODO(brians): static_assert that this is at the front
- uint32_t checksum;
-
- Values values;
- // Starts at 0 and goes up.
- int32_t count;
-
- void NetworkToHost() {
- count = ntoh(count);
- }
- void HostToNetwork() {
- count = hton(count);
- }
-
- void FillinChecksum() {
- checksum = CalculateChecksum(reinterpret_cast<char *>(this) +
- sizeof(checksum),
- sizeof(*this) - sizeof(checksum));
- }
- // Returns whether or not checksum is correct.
- bool CheckChecksum() {
- uint32_t expected = CalculateChecksum(reinterpret_cast<char *>(this) +
- sizeof(checksum),
- sizeof(*this) - sizeof(checksum));
- if (checksum != expected) {
- LOG(INFO, "expected %" PRIx32 " but got %" PRIx32 "\n",
- expected, checksum);
- return false;
- }
- return true;
- }
-} __attribute__((packed));
-
-} // namespace sensors
-} // namespace aos
-
-#endif // AOS_COMMON_SENSORS_SENSORS_H_
diff --git a/aos/common/sensors/sensors_test.cc b/aos/common/sensors/sensors_test.cc
deleted file mode 100644
index 0df93c9..0000000
--- a/aos/common/sensors/sensors_test.cc
+++ /dev/null
@@ -1,38 +0,0 @@
-#include "aos/common/sensors/sensors.h"
-
-#include <stdint.h>
-
-#include "gtest/gtest.h"
-
-#include "aos/common/queue_testutils.h"
-
-namespace aos {
-namespace sensors {
-namespace testing {
-
-struct TestValues {
- int32_t data1, data2;
-};
-
-TEST(SensorDataTest, Checksum) {
- ::aos::common::testing::EnableTestLogging();
-
- SensorData<TestValues> data;
- data.values.data1 = 0;
- data.values.data2 = 5;
- data.FillinChecksum();
- EXPECT_TRUE(data.CheckChecksum());
- data.values.data1 = 1;
- EXPECT_FALSE(data.CheckChecksum());
- data.values.data1 = 0xFFFFFFFF;
- EXPECT_FALSE(data.CheckChecksum());
- data.values.data1 = 0;
- EXPECT_TRUE(data.CheckChecksum());
- data.values.data1 = 5;
- data.values.data2 = 0;
- EXPECT_FALSE(data.CheckChecksum());
-}
-
-} // namespace testing
-} // namespace sensors
-} // namespace aos
diff --git a/aos/common/time.cc b/aos/common/time.cc
index c4f77a8..ced0c4d 100644
--- a/aos/common/time.cc
+++ b/aos/common/time.cc
@@ -32,7 +32,7 @@
// That would let me create a MockTime clock source.
}
-void Time::EnableMockTime(const Time now) {
+void Time::EnableMockTime(const Time &now) {
mock_time_enabled = true;
MutexLocker time_mutex_locker(&time_mutex);
current_mock_time = now;
@@ -43,7 +43,7 @@
mock_time_enabled = false;
}
-void Time::SetMockTime(const Time now) {
+void Time::SetMockTime(const Time &now) {
MutexLocker time_mutex_locker(&time_mutex);
if (!mock_time_enabled) {
LOG(FATAL, "Tried to set mock time and mock time is not enabled\n");
@@ -51,6 +51,12 @@
current_mock_time = now;
}
+void Time::IncrementMockTime(const Time &amount) {
+ static ::aos::Mutex mutex;
+ ::aos::MutexLocker sync(&mutex);
+ SetMockTime(Now() + amount);
+}
+
Time Time::Now(clockid_t clock) {
if (mock_time_enabled) {
MutexLocker time_mutex_locker(&time_mutex);
@@ -58,9 +64,6 @@
} else {
timespec temp;
if (clock_gettime(clock, &temp) != 0) {
- // TODO(aschuh): There needs to be a pluggable low level logging interface
- // so we can break this dependency loop. This also would help during
- // startup.
LOG(FATAL, "clock_gettime(%jd, %p) failed with %d: %s\n",
static_cast<uintmax_t>(clock), &temp, errno, strerror(errno));
}
@@ -107,6 +110,10 @@
sec_ *= rhs; // better not overflow, or the result is just too big
nsec_ = temp % kNSecInSec;
sec_ += (temp - nsec_) / kNSecInSec;
+ if (nsec_ < 0) {
+ nsec_ += kNSecInSec;
+ sec_ -= 1;
+ }
return *this;
}
const Time Time::operator*(int32_t rhs) const {
@@ -115,6 +122,10 @@
Time &Time::operator/=(int32_t rhs) {
nsec_ = (sec_ % rhs) * (kNSecInSec / rhs) + nsec_ / rhs;
sec_ /= rhs;
+ if (nsec_ < 0) {
+ nsec_ += kNSecInSec;
+ sec_ -= 1;
+ }
return *this;
}
const Time Time::operator/(int32_t rhs) const {
@@ -128,12 +139,20 @@
const int wraps = nsec_ / kNSecInSec;
sec_ = wraps;
nsec_ -= kNSecInSec * wraps;
+ if (nsec_ < 0) {
+ nsec_ += kNSecInSec;
+ sec_ -= 1;
+ }
return *this;
}
const Time Time::operator%(int32_t rhs) const {
return Time(*this) %= rhs;
}
+const Time Time::operator-() const {
+ return Time(-sec_ - 1, kNSecInSec - nsec_);
+}
+
bool Time::operator==(const Time &rhs) const {
return sec_ == rhs.sec_ && nsec_ == rhs.nsec_;
}
diff --git a/aos/common/time.h b/aos/common/time.h
index 6c80451..bc2dc3e 100644
--- a/aos/common/time.h
+++ b/aos/common/time.h
@@ -23,16 +23,22 @@
// 0 <= nsec_ < kNSecInSec should always be true. All functions here will make
// sure that that is true if it was on all inputs (including *this).
//
+// Negative times are supported so that all of the normal arithmetic identities
+// work. nsec_ is still always positive.
+//
// The arithmetic and comparison operators are overloaded because they make
// complete sense and are very useful. The default copy and assignment stuff is
-// left because it works fine. Multiplication and division of Times by Times are
+// left because it works fine. Multiplication of Times by Times is
// not implemented because I can't think of any uses for them and there are
-// multiple ways to do it.
+// multiple ways to do it. Division of Times by Times is implemented as the
+// ratio of them. Multiplication, division, and modulus of Times by integers are
+// implemented as interpreting the argument as nanoseconds.
struct Time {
#ifdef SWIG
// All of the uses of constexpr here can safely be simply removed.
// NOTE: This means that relying on the fact that constexpr implicitly makes
-// member functions const is not safe.
+// member functions const is not valid, so they all have to be explicitly marked
+// const.
#define constexpr
#endif // SWIG
public:
@@ -83,7 +89,6 @@
static Time Now(clockid_t clock = kDefaultClock);
// Constructs a Time representing seconds.
- // TODO(brians): fix and test the negative cases for all of these
static constexpr Time InSeconds(double seconds) {
return (seconds < 0.0) ?
Time(static_cast<int32_t>(seconds) - 1,
@@ -94,8 +99,11 @@
// Constructs a time representing microseconds.
static constexpr Time InNS(int64_t nseconds) {
- return Time(nseconds / static_cast<int64_t>(kNSecInSec),
- nseconds % kNSecInSec);
+ return (nseconds < 0) ?
+ Time(nseconds / static_cast<int64_t>(kNSecInSec) - 1,
+ (nseconds % kNSecInSec) + kNSecInSec) :
+ Time(nseconds / static_cast<int64_t>(kNSecInSec),
+ nseconds % kNSecInSec);
}
// Constructs a time representing microseconds.
@@ -108,7 +116,10 @@
// Constructs a time representing mseconds.
static constexpr Time InMS(int mseconds) {
- return Time(mseconds / kMSecInSec, (mseconds % kMSecInSec) * kNSecInMSec);
+ return (mseconds < 0) ?
+ Time(mseconds / kMSecInSec - 1,
+ (mseconds % kMSecInSec) * kNSecInMSec + kNSecInSec) :
+ Time(mseconds / kMSecInSec, (mseconds % kMSecInSec) * kNSecInMSec);
}
// Checks whether or not this time is within amount nanoseconds of other.
@@ -138,7 +149,6 @@
}
// Returns the time represent in microseconds.
- // TODO(brians): test this
int64_t constexpr ToUSec() const {
return static_cast<int64_t>(sec_) * static_cast<int64_t>(kUSecInSec) +
(static_cast<int64_t>(nsec_) / static_cast<int64_t>(kNSecInUSec));
@@ -155,15 +165,22 @@
Time &operator*=(int32_t rhs);
Time &operator/=(int32_t rhs);
Time &operator%=(int32_t rhs);
+ Time &operator%=(double rhs) = delete;
+ Time &operator*=(double rhs) = delete;
+ Time &operator/=(double rhs) = delete;
+ const Time operator*(double rhs) const = delete;
+ const Time operator/(double rhs) const = delete;
+ const Time operator%(double rhs) const = delete;
#endif
const Time operator+(const Time &rhs) const;
const Time operator-(const Time &rhs) const;
const Time operator*(int32_t rhs) const;
const Time operator/(int32_t rhs) const;
- // TODO(brians) test this
double operator/(const Time &rhs) const;
const Time operator%(int32_t rhs) const;
+ const Time operator-() const;
+
bool operator==(const Time &rhs) const;
bool operator!=(const Time &rhs) const;
bool operator<(const Time &rhs) const;
@@ -194,14 +211,12 @@
// Enables returning the mock time value for Now instead of checking the
// system clock. This should only be used when testing things depending on
// time, or many things may/will break.
- static void EnableMockTime(const Time now);
+ static void EnableMockTime(const Time &now);
// Sets now when time is being mocked.
- static void SetMockTime(const Time now);
- // Convenience function to just increment the mock time by a certain amount.
- static void IncrementMockTime(const Time amount) {
- // TODO(brians) make this thread safe so it's more useful?
- SetMockTime(Now() + amount);
- }
+ static void SetMockTime(const Time &now);
+ // Convenience function to just increment the mock time by a certain amount in
+ // a thread safe way.
+ static void IncrementMockTime(const Time &amount);
// Disables mocking time.
static void DisableMockTime();
@@ -212,7 +227,9 @@
static void CheckImpl(int32_t nsec);
void Check() { CheckImpl(nsec_); }
// A constexpr version of CheckImpl that returns the given value when it
- // succeeds.
+ // succeeds or evaluates to non-constexpr and returns 0 when it fails.
+ // This will result in the usual LOG(FATAL) if this is used where it isn't
+ // required to be constexpr or a compile error if it is.
static constexpr int32_t CheckConstexpr(int32_t nsec) {
return (nsec >= kNSecInSec || nsec < 0) ? CheckImpl(nsec), 0 : nsec;
}
diff --git a/aos/common/time_test.cc b/aos/common/time_test.cc
index e6b7bb0..ebc8284 100644
--- a/aos/common/time_test.cc
+++ b/aos/common/time_test.cc
@@ -56,6 +56,9 @@
EXPECT_EQ(MACRO_DARG(Time(57, 6500)), t + MACRO_DARG(Time(3, 6000)));
EXPECT_EQ(MACRO_DARG(Time(50, 300)),
t + MACRO_DARG(Time(-5, Time::kNSecInSec - 200)));
+ EXPECT_EQ(Time(-46, 500), t + Time(-100, 0));
+ EXPECT_EQ(Time(-47, Time::kNSecInSec - 500),
+ Time(-101, Time::kNSecInSec - 1000) + t);
}
TEST(TimeTest, Subtraction) {
Time t(54, 500);
@@ -66,28 +69,61 @@
t - MACRO_DARG(Time(0, Time::kNSecInSec - 100)));
EXPECT_EQ(MACRO_DARG(Time(55, 800)),
t - MACRO_DARG(Time(-2, Time::kNSecInSec - 300)));
+ EXPECT_EQ(Time(54, 5500), t - Time(-1, Time::kNSecInSec - 5000));
+ EXPECT_EQ(Time(-50, Time::kNSecInSec - 300),
+ Time(5, 200) - t);
}
TEST(TimeTest, Multiplication) {
Time t(54, Time::kNSecInSec / 3);
EXPECT_EQ(MACRO_DARG(Time(108, Time::kNSecInSec / 3 * 2)), t * 2);
EXPECT_EQ(MACRO_DARG(Time(271, Time::kNSecInSec / 3 * 2 - 1)), t * 5);
+ EXPECT_EQ(Time(-109, Time::kNSecInSec / 3 + 1), t * -2);
+ EXPECT_EQ(Time(-55, Time::kNSecInSec / 3 * 2 + 1), t * -1);
+ EXPECT_EQ(Time(-218, Time::kNSecInSec / 3 * 2 + 2), (t * -1) * 4);
}
-TEST(TimeTest, Division) {
- EXPECT_EQ(MACRO_DARG(Time(5, Time::kNSecInSec / 10 * 4 + 50)),
- MACRO_DARG(Time(54, 500)) / 10);
+TEST(TimeTest, DivisionByInt) {
+ EXPECT_EQ(Time(5, Time::kNSecInSec / 10 * 4 + 50), Time(54, 500) / 10);
+ EXPECT_EQ(Time(2, Time::kNSecInSec / 4 * 3),
+ Time(5, Time::kNSecInSec / 2) / 2);
+ EXPECT_EQ(Time(-3, Time::kNSecInSec / 4 * 3),
+ Time(-5, Time::kNSecInSec / 2) / 2);
+}
+TEST(TimeTest, DivisionByTime) {
+ EXPECT_DOUBLE_EQ(2, Time(10, 0) / Time(5, 0));
+ EXPECT_DOUBLE_EQ(9, Time(27, 0) / Time(3, 0));
+ EXPECT_DOUBLE_EQ(9.25, Time(37, 0) / Time(4, 0));
+ EXPECT_DOUBLE_EQ(5.25, Time(36, Time::kNSecInSec / 4 * 3) / Time(7, 0));
+ EXPECT_DOUBLE_EQ(-5.25, Time(-37, Time::kNSecInSec / 4) / Time(7, 0));
+ EXPECT_DOUBLE_EQ(-5.25, Time(36, Time::kNSecInSec / 4 * 3) / Time(-7, 0));
+}
+
+TEST(TimeTest, Negation) {
+ EXPECT_EQ(Time(-5, 1234), -Time(4, Time::kNSecInSec - 1234));
+ EXPECT_EQ(Time(5, Time::kNSecInSec * 2 / 3 + 1),
+ -Time(-6, Time::kNSecInSec / 3));
}
TEST(TimeTest, Comparisons) {
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) > Time(971, 253)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) >= Time(971, 253)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) < Time(971, 255)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) <= Time(971, 255)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) >= Time(971, 253)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) <= Time(971, 254)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) >= Time(971, 254)));
- EXPECT_TRUE(MACRO_DARG(Time(972, 254) > Time(971, 254)));
- EXPECT_TRUE(MACRO_DARG(Time(971, 254) < Time(972, 254)));
+ EXPECT_TRUE(Time(971, 254) > Time(971, 253));
+ EXPECT_TRUE(Time(971, 254) >= Time(971, 253));
+ EXPECT_TRUE(Time(971, 254) < Time(971, 255));
+ EXPECT_TRUE(Time(971, 254) <= Time(971, 255));
+ EXPECT_TRUE(Time(971, 254) >= Time(971, 253));
+ EXPECT_TRUE(Time(971, 254) <= Time(971, 254));
+ EXPECT_TRUE(Time(971, 254) >= Time(971, 254));
+ EXPECT_TRUE(Time(972, 254) > Time(971, 254));
+ EXPECT_TRUE(Time(971, 254) < Time(972, 254));
+
+ EXPECT_TRUE(Time(-971, 254) > Time(-971, 253));
+ EXPECT_TRUE(Time(-971, 254) >= Time(-971, 253));
+ EXPECT_TRUE(Time(-971, 254) < Time(-971, 255));
+ EXPECT_TRUE(Time(-971, 254) <= Time(-971, 255));
+ EXPECT_TRUE(Time(-971, 254) >= Time(-971, 253));
+ EXPECT_TRUE(Time(-971, 254) <= Time(-971, 254));
+ EXPECT_TRUE(Time(-971, 254) >= Time(-971, 254));
+ EXPECT_TRUE(Time(-972, 254) < Time(-971, 254));
+ EXPECT_TRUE(Time(-971, 254) > Time(-972, 254));
}
TEST(TimeTest, Within) {
@@ -95,54 +131,73 @@
EXPECT_FALSE(MACRO_DARG(Time(55, 5000).IsWithin(Time(55, 4900), 99)));
EXPECT_TRUE(MACRO_DARG(Time(5, 0).IsWithin(Time(4, Time::kNSecInSec - 200),
250)));
+ EXPECT_TRUE(Time(-5, Time::kNSecInSec - 200).IsWithin(Time(-4, 0), 250));
+ EXPECT_TRUE(Time(-5, 200).IsWithin(Time(-5, 0), 250));
}
-TEST(TimeTest, Modulo) {
+TEST(TimeTest, Modulus) {
EXPECT_EQ(MACRO_DARG(Time(0, Time::kNSecInSec / 10 * 2)),
MACRO_DARG(Time(50, 0) % (Time::kNSecInSec / 10 * 3)));
+ EXPECT_EQ(Time(-1, Time::kNSecInSec / 10 * 8),
+ Time(-50, 0) % (Time::kNSecInSec / 10 * 3));
+ EXPECT_EQ(Time(-1, Time::kNSecInSec / 10 * 8),
+ Time(-50, 0) % (-Time::kNSecInSec / 10 * 3));
+ EXPECT_EQ(Time(0, Time::kNSecInSec / 10 * 2),
+ Time(50, 0) % (-Time::kNSecInSec / 10 * 3));
}
+// TODO(brians): Finish tests for negatives from here on.
TEST(TimeTest, InSeconds) {
EXPECT_EQ(MACRO_DARG(Time(2, Time::kNSecInSec / 100 * 55 - 1)),
Time::InSeconds(2.55));
+ EXPECT_EQ(MACRO_DARG(Time(-3, Time::kNSecInSec / 100 * 45)),
+ Time::InSeconds(-2.55));
}
TEST(TimeTest, ToSeconds) {
- EXPECT_EQ(13.23, Time::InSeconds(13.23).ToSeconds());
+ EXPECT_DOUBLE_EQ(13.23, Time::InSeconds(13.23).ToSeconds());
+ EXPECT_NEAR(-13.23, Time::InSeconds(-13.23).ToSeconds(),
+ 1.0 / Time::kNSecInSec * 2);
}
-#ifdef __VXWORKS__
-TEST(TimeTest, ToTicks) {
- EXPECT_EQ(sysClkRateGet() / 100,
- MACRO_DARG(Time(0, Time::kNSecInSec / 100).ToTicks()));
-}
-TEST(TimeTest, InTicks) {
- EXPECT_EQ(MACRO_DARG(Time(2, Time::kNSecInSec)),
- Time::InTicks(sysClkRateGet() * 2.5));
-}
-#endif
-
TEST(TimeTest, InMS) {
Time t = Time::InMS(254971);
EXPECT_EQ(254, t.sec());
EXPECT_EQ(971000000, t.nsec());
+
+ Time t2 = Time::InMS(-254971);
+ EXPECT_EQ(-255, t2.sec());
+ EXPECT_EQ(Time::kNSecInSec - 971000000, t2.nsec());
+}
+
+TEST(TimeTest, ToMSec) {
+ EXPECT_EQ(254971, Time(254, 971000000).ToMSec());
+ EXPECT_EQ(-254971, Time(-255, Time::kNSecInSec - 971000000).ToMSec());
}
TEST(TimeTest, InNS) {
Time t = Time::InNS(static_cast<int64_t>(973254111971ll));
EXPECT_EQ(973, t.sec());
EXPECT_EQ(254111971, t.nsec());
+
+ Time t2 = Time::InNS(static_cast<int64_t>(-973254111971ll));
+ EXPECT_EQ(-974, t2.sec());
+ EXPECT_EQ(Time::kNSecInSec - 254111971, t2.nsec());
}
TEST(TimeTest, InUS) {
Time t = Time::InUS(254111971);
EXPECT_EQ(254, t.sec());
EXPECT_EQ(111971000, t.nsec());
+
+ Time t2 = Time::InUS(-254111971);
+ EXPECT_EQ(-255, t2.sec());
+ EXPECT_EQ(Time::kNSecInSec - 111971000, t2.nsec());
}
-TEST(TimeTest, ToMSec) {
- Time t(254, 971000000);
- EXPECT_EQ(254971, t.ToMSec());
+TEST(TimeTest, ToUSec) {
+ EXPECT_EQ(254000971, Time(254, 971000).ToUSec());
+ EXPECT_EQ(-254000971, Time(-255, Time::kNSecInSec - 971000).ToUSec());
}
TEST(TimeTest, Abs) {
diff --git a/aos/common/util/util.gyp b/aos/common/util/util.gyp
index e295682..852f3d7 100644
--- a/aos/common/util/util.gyp
+++ b/aos/common/util/util.gyp
@@ -41,5 +41,23 @@
'<(AOS)/build/aos.gyp:logging',
],
},
+ {
+ 'target_name': 'wrapping_counter',
+ 'type': 'static_library',
+ 'sources': [
+ 'wrapping_counter.cc',
+ ],
+ },
+ {
+ 'target_name': 'wrapping_counter_test',
+ 'type': 'executable',
+ 'sources': [
+ 'wrapping_counter_test.cc',
+ ],
+ 'dependencies': [
+ 'wrapping_counter',
+ '<(EXTERNALS):gtest',
+ ],
+ },
],
}
diff --git a/aos/common/util/wrapping_counter.cc b/aos/common/util/wrapping_counter.cc
new file mode 100644
index 0000000..61f5047
--- /dev/null
+++ b/aos/common/util/wrapping_counter.cc
@@ -0,0 +1,19 @@
+#include "aos/common/util/wrapping_counter.h"
+
+namespace aos {
+namespace util {
+
+WrappingCounter::WrappingCounter(int32_t initial_count)
+ : count_(initial_count), last_count_(0) {}
+
+int32_t WrappingCounter::Update(uint8_t current) {
+ if (last_count_ > current) {
+ count_ += 0x100;
+ }
+ count_ = (count_ & 0xffffff00) | current;
+ last_count_ = current;
+ return count_;
+}
+
+} // namespace util
+} // namespace aos
diff --git a/aos/common/util/wrapping_counter.h b/aos/common/util/wrapping_counter.h
new file mode 100644
index 0000000..fbf3611
--- /dev/null
+++ b/aos/common/util/wrapping_counter.h
@@ -0,0 +1,34 @@
+#ifndef AOS_COMMON_UTIL_WRAPPING_COUNTER_H_
+#define AOS_COMMON_UTIL_WRAPPING_COUNTER_H_
+
+#include <stdint.h>
+
+namespace aos {
+namespace util {
+
+// Deals correctly with 1-byte counters which wrap.
+// This is only possible if the counter never wraps twice between Update calls.
+// It will also fail if the counter ever goes down (that will be interpreted as
+// +255 instead of -1, for example).
+class WrappingCounter {
+ public:
+ WrappingCounter(int32_t initial_count = 0);
+
+ // Updates the internal counter with a new raw value.
+ // Returns count() for convenience.
+ int32_t Update(uint8_t current);
+
+ // Resets the actual count to value.
+ void Reset(int32_t value = 0) { count_ = value; }
+
+ int32_t count() const { return count_; }
+
+ private:
+ int32_t count_;
+ uint8_t last_count_;
+};
+
+} // namespace util
+} // namespace aos
+
+#endif // AOS_COMMON_UTIL_WRAPPING_COUNTER_H_
diff --git a/aos/common/util/wrapping_counter_test.cc b/aos/common/util/wrapping_counter_test.cc
new file mode 100644
index 0000000..e257fb0
--- /dev/null
+++ b/aos/common/util/wrapping_counter_test.cc
@@ -0,0 +1,58 @@
+#include "aos/common/util/wrapping_counter.h"
+
+#include <limits.h>
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace util {
+namespace testing {
+
+TEST(WrappingCounterTest, Basic) {
+ WrappingCounter test_counter;
+ EXPECT_EQ(0, test_counter.count());
+ EXPECT_EQ(1, test_counter.Update(1));
+ EXPECT_EQ(1, test_counter.Update(1));
+ EXPECT_EQ(2, test_counter.Update(2));
+ EXPECT_EQ(7, test_counter.Update(7));
+ EXPECT_EQ(7, test_counter.count());
+ EXPECT_EQ(123, test_counter.Update(123));
+ EXPECT_EQ(123, test_counter.count());
+}
+
+TEST(WrappingCounterTest, Reset) {
+ WrappingCounter test_counter;
+ test_counter.Update(5);
+ test_counter.Reset();
+ EXPECT_EQ(0, test_counter.count());
+ test_counter.Reset(56);
+ EXPECT_EQ(56, test_counter.count());
+}
+
+namespace {
+void test_wrapping(int16_t start, int16_t step) {
+ WrappingCounter test_counter;
+ for (int16_t i = start; i < INT16_MAX - step; i += step) {
+ EXPECT_EQ(i, test_counter.Update(i & 0xFF));
+ }
+}
+}
+
+// This tests the basic wrapping functionality.
+TEST(WrappingCounterTest, ReasonableWrapping) {
+ test_wrapping(0, 13);
+ test_wrapping(0, 53);
+ test_wrapping(0, 64);
+ test_wrapping(0, 73);
+}
+
+// It would be reasonable for these to fail if the implementation changes.
+TEST(WrappingCounterTest, UnreasonableWrapping) {
+ test_wrapping(0, 128);
+ test_wrapping(0, 213);
+ test_wrapping(0, 255);
+}
+
+} // namespace testing
+} // namespace util
+} // namespace aos
diff --git a/aos/externals/.gitignore b/aos/externals/.gitignore
index 80bd460..73cb3e2 100644
--- a/aos/externals/.gitignore
+++ b/aos/externals/.gitignore
@@ -7,13 +7,13 @@
/gccdist/
/gtest-1.6.0-p1/
/gtest-1.6.0.zip
-/gyp-1488/
+/gyp-1738/
/javacv-0.2-bin.zip
/javacv-bin/
/jpeg-8d/
/jpegsrc.v8d.tar.gz
/libjpeg/
-/ninja/
+/ninja-v1.4.0/
/one-jar-boot-0.97.jar
/gflags-2.0-prefix/
/gflags-2.0.tar.gz
diff --git a/aos/externals/gyp.patch b/aos/externals/gyp.patch
index 9019406..b09b67d 100644
--- a/aos/externals/gyp.patch
+++ b/aos/externals/gyp.patch
@@ -1,7 +1,7 @@
diff -rupN before/pylib/gyp/input.py after/pylib/gyp/input.py
--- before/pylib/gyp/input.py 2012-11-20 16:38:09.394784918 -0800
+++ after/pylib/gyp/input.py 2012-11-20 16:39:10.527105964 -0800
-@@ -2156,17 +2156,6 @@ def ValidateSourcesInTarget(target, targ
+@@ -2412,17 +2412,6 @@ def ValidateSourcesInTarget(target, targ
basename = os.path.basename(name) # Don't include extension.
basenames.setdefault(basename, []).append(source)
@@ -11,10 +11,10 @@
- error += ' %s: %s\n' % (basename, ' '.join(files))
-
- if error:
-- print ('static library %s has several files with the same basename:\n' %
-- target + error + 'Some build systems, e.g. MSVC08, '
-- 'cannot handle that.')
-- raise KeyError, 'Duplicate basenames in sources section, see list above'
+- print('static library %s has several files with the same basename:\n' %
+- target + error + 'Some build systems, e.g. MSVC08, '
+- 'cannot handle that.')
+- raise GypError('Duplicate basenames in sources section, see list above')
-
def ValidateRulesInTarget(target, target_dict, extra_sources_for_rules):
diff --git a/bot3/input/gyro_reader.cc b/bot3/input/gyro_reader.cc
deleted file mode 100644
index bdcd07e..0000000
--- a/bot3/input/gyro_reader.cc
+++ /dev/null
@@ -1,51 +0,0 @@
-#include <unistd.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-
-#include "aos/common/logging/logging.h"
-#include "aos/atom_code/init.h"
-
-#include "frc971/queues/GyroAngle.q.h"
-
-#define M_PI 3.14159265358979323846264338327
-
-using frc971::sensors::gyro;
-
-int main(){
- aos::Init();
- int fd = open("/dev/aschuh0", O_RDONLY);
- int rate_limit = 0;
- if (fd < 0) {
- LOG(ERROR, "No Gyro found.\n");
- } else {
- LOG(INFO, "Gyro now connected\n");
- }
-
- while (true) {
- int64_t gyro_value;
- if (read(fd, (void *)&gyro_value, sizeof(gyro_value)) != sizeof(gyro_value)) {
- LOG(ERROR, "Could not read gyro errno: %d\n", errno);
- if (errno == ENODEV || errno == EBADF) {
- close(fd);
- while (1) {
- usleep(1000);
- fd = open("/dev/aschuh0", O_RDONLY);
- if (fd > 0) {
- LOG(INFO, "Found gyro again\n");
- break;
- }
- }
- }
- continue;
- }
- rate_limit ++;
- if (rate_limit > 10) {
- LOG(DEBUG, "Gyro is %d\n", (int)(gyro_value / 16));
- rate_limit = 0;
- }
- gyro.MakeWithBuilder().angle(gyro_value / 16.0 / 1000.0 / 180.0 * M_PI).Send();
- }
-
- aos::Cleanup();
-}
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 99f7047..a2bdc8d 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -18,14 +18,12 @@
'../control_loops/shooter/shooter.gyp:shooter',
'../autonomous/autonomous.gyp:auto',
'../input/input.gyp:joystick_reader',
- '../input/input.gyp:gyro_reader',
'../../vision/vision.gyp:OpenCVWorkTask',
'../../vision/vision.gyp:GoalMaster',
'../output/output.gyp:MotorWriter',
'../output/output.gyp:CameraServer',
#'camera/camera.gyp:frc971',
'../../gyro_board/src/libusb-driver/libusb-driver.gyp:get',
- '../input/input.gyp:gyro_board_reader',
'../input/input.gyp:gyro_sensor_receiver',
],
'copies': [
diff --git a/frc971/atom_code/build.sh b/frc971/atom_code/build.sh
index 43dd4bf..b8129ab 100755
--- a/frc971/atom_code/build.sh
+++ b/frc971/atom_code/build.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-../../aos/build/build.sh atom atom_code.gyp no atom $1
+../../aos/build/build.sh atom atom_code.gyp no "$@"
diff --git a/frc971/atom_code/scripts/start_list.txt b/frc971/atom_code/scripts/start_list.txt
index 13c9b99..8b51057 100644
--- a/frc971/atom_code/scripts/start_list.txt
+++ b/frc971/atom_code/scripts/start_list.txt
@@ -8,4 +8,4 @@
index
shooter
auto
-gyro_board_reader
+gyro_sensor_receiver
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index e7c48a5..13aeb51 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -421,8 +421,6 @@
SetDriveGoal(kDistanceToCenterMeters);
if (ShouldExitAuto()) return;
- return;
-
ShootNDiscs(4);
if (ShouldExitAuto()) return;
} else {
@@ -441,11 +439,11 @@
WaitForIndex(); // ready to pick up discs
// How long we're going to drive in total.
- static const double kDriveDistance = 2.8;
+ static const double kDriveDistance = 2.84;
// How long to drive slowly to pick up the 2 disks under the pyramid.
static const double kFirstDrive = 0.4;
// How long to drive slowly to pick up the last 2 disks.
- static const double kLastDrive = 0.3;
+ static const double kLastDrive = 0.34;
// How fast to drive when picking up disks.
static const double kPickupVelocity = 0.6;
// Where to take the second set of shots from.
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 65fb216..3016db5 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -17,8 +17,10 @@
namespace {
// It has about 0.029043 of gearbox slop.
+// For purposes of moving the end up/down by a certain amount, the wrist is 18
+// inches long.
const double kCompWristHallEffectStartAngle = 1.285;
-const double kPracticeWristHallEffectStartAngle = 1.164;
+const double kPracticeWristHallEffectStartAngle = 1.175;
const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
diff --git a/frc971/constants.h b/frc971/constants.h
index a1fc749..414d10d 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -10,8 +10,8 @@
// store their output value into and assume that aos::robot_state->get() is
// not null and is correct. They return true on success.
-const uint16_t kCompTeamNumber = 971;
-const uint16_t kPracticeTeamNumber = 5971;
+const uint16_t kCompTeamNumber = 5971;
+const uint16_t kPracticeTeamNumber = 971;
// Sets *angle to how many radians from horizontal to the location of interest.
bool wrist_hall_effect_start_angle(double *angle);
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 7b07247..244f3e5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -8,13 +8,13 @@
namespace control_loops {
class DrivetrainLoop
- : public aos::control_loops::ControlLoop<control_loops::Drivetrain> {
+ : public aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false> {
public:
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
control_loops::Drivetrain *my_drivetrain = &control_loops::drivetrain)
- : aos::control_loops::ControlLoop<control_loops::Drivetrain>(
+ : aos::control_loops::ControlLoop<control_loops::Drivetrain, true, false>(
my_drivetrain) {}
protected:
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index 3932ab5..6ac91b7 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -54,8 +54,10 @@
loader_up_(false),
disc_clamped_(false),
disc_ejected_(false),
+ is_shooting_(false),
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
+ hopper_clear_(true),
no_prior_position_(true),
missing_position_count_(0) {
}
@@ -107,9 +109,10 @@
/*static*/ const int IndexMotor::kGrabbingDelay = 5;
/*static*/ const int IndexMotor::kLiftingDelay = 2;
-/*static*/ const int IndexMotor::kLiftingTimeout = 65;
+/*static*/ const int IndexMotor::kLiftingTimeout = 100;
/*static*/ const int IndexMotor::kShootingDelay = 10;
-/*static*/ const int IndexMotor::kLoweringDelay = 20;
+/*static*/ const int IndexMotor::kLoweringDelay = 4;
+/*static*/ const int IndexMotor::kLoweringTimeout = 120;
// TODO(aschuh): Tune these.
/*static*/ const double
@@ -361,7 +364,7 @@
}
if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
- LOG(INFO, "Saw a posedge\n");
+ LOG(INFO, "Saw a top posedge\n");
const double index_position = wrist_loop_->X_hat(0, 0) -
position->index_position + position->top_disc_posedge_position;
// TODO(aschuh): Sanity check this number...
@@ -533,6 +536,7 @@
break;
case Goal::READY_LOWER:
case Goal::INTAKE:
+ hopper_clear_ = false;
{
if (position) {
// Posedge of the disc entering the beam break.
@@ -689,6 +693,7 @@
if (loader_state_ == LoaderState::GRABBED &&
safe_goal_ == Goal::SHOOT) {
loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ is_shooting_ = true;
}
// Must wait until it has been grabbed to continue.
@@ -757,6 +762,7 @@
"Emptied the hopper out but there are still discs there\n");
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
}
@@ -789,6 +795,7 @@
hopper_disc_count_);
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
@@ -832,6 +839,7 @@
loader_up_ = false;
disc_clamped_ = false;
disc_ejected_ = false;
+ disc_stuck_in_loader_ = false;
if (loader_goal_ == LoaderGoal::GRAB ||
loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
if (goal->force_fire) {
@@ -865,8 +873,7 @@
disc_clamped_ = true;
disc_ejected_ = false;
if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
- shooter.status.FetchLatest();
- if (shooter.status.get()) {
+ if (shooter.status.FetchLatest() || shooter.status.get()) {
// TODO(aschuh): If we aren't shooting nicely, wait until the shooter
// is up to speed rather than just spinning.
if (shooter.status->average_velocity > 130 && shooter.status->ready) {
@@ -892,7 +899,7 @@
break;
}
case LoaderState::LIFTING:
- LOG(DEBUG, "Loader LIFTING %d\n", loader_countdown_);
+ LOG(DEBUG, "Loader LIFTING %d %d\n", loader_countdown_, loader_timeout_);
// Lifting the disc.
loader_up_ = true;
disc_clamped_ = true;
@@ -900,6 +907,8 @@
if (position->loader_top) {
if (loader_countdown_ > 0) {
--loader_countdown_;
+ loader_timeout_ = 0;
+ break;
} else {
loader_state_ = LoaderState::LIFTED;
}
@@ -909,10 +918,14 @@
++loader_timeout_;
if (loader_timeout_ > kLiftingTimeout) {
LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
- loader_state_ = LoaderState::LIFTED;
+ loader_state_ = LoaderState::LOWERING;
+ loader_countdown_ = kLoweringDelay;
+ loader_timeout_ = 0;
+ disc_stuck_in_loader_ = true;
+ } else {
+ break;
}
}
- break;
case LoaderState::LIFTED:
LOG(DEBUG, "Loader LIFTED\n");
// Disc lifted. Time to eject it out.
@@ -941,30 +954,53 @@
disc_ejected_ = true;
loader_state_ = LoaderState::LOWERING;
loader_countdown_ = kLoweringDelay;
- --hopper_disc_count_;
- ++shot_disc_count_;
+ loader_timeout_ = 0;
case LoaderState::LOWERING:
- LOG(DEBUG, "Loader LOWERING %d\n", loader_countdown_);
+ LOG(DEBUG, "Loader LOWERING %d %d\n", loader_countdown_, loader_timeout_);
// Lowering the loader back down.
loader_up_ = false;
disc_clamped_ = false;
- disc_ejected_ = true;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
+ // We don't want to eject if we're stuck because it will force the disc
+ // into the green loader wheel.
+ disc_ejected_ = disc_stuck_in_loader_ ? false : true;
+ if (position->loader_bottom) {
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ loader_timeout_ = 0;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ --hopper_disc_count_;
+ ++shot_disc_count_;
+ }
} else {
- loader_state_ = LoaderState::LOWERED;
+ // Restart the countdown if it bounces back up or something.
+ loader_countdown_ = kLoweringDelay;
+ ++loader_timeout_;
+ if (loader_timeout_ > kLoweringTimeout) {
+ LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
+ loader_state_ = LoaderState::LOWERED;
+ disc_stuck_in_loader_ = true;
+ } else {
+ break;
+ }
}
case LoaderState::LOWERED:
LOG(DEBUG, "Loader LOWERED\n");
- // The indexer is lowered.
loader_up_ = false;
- disc_clamped_ = false;
disc_ejected_ = false;
- loader_state_ = LoaderState::READY;
- // Once we have shot, we need to hang out in READY until otherwise
- // notified.
- loader_goal_ = LoaderGoal::READY;
+ is_shooting_ = false;
+ if (disc_stuck_in_loader_) {
+ disc_stuck_in_loader_ = false;
+ disc_clamped_ = true;
+ loader_state_ = LoaderState::GRABBED;
+ } else {
+ disc_clamped_ = false;
+ loader_state_ = LoaderState::READY;
+ // Once we have shot, we need to hang out in READY until otherwise
+ // notified.
+ loader_goal_ = LoaderGoal::READY;
+ }
break;
}
@@ -1007,10 +1043,16 @@
status->total_disc_count = total_disc_count_;
status->shot_disc_count = shot_disc_count_;
status->preloaded = (loader_state_ != LoaderState::READY);
+ status->is_shooting = is_shooting_;
+ status->hopper_clear = hopper_clear_;
if (output) {
output->intake_voltage = intake_voltage;
- output->transfer_voltage = transfer_voltage;
+ if (goal->override_transfer) {
+ output->transfer_voltage = goal->transfer_voltage;
+ } else {
+ output->transfer_voltage = transfer_voltage;
+ }
if (goal->override_index) {
output->index_voltage = goal->index_voltage;
} else {
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index 8abb119..e6902d8 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -143,8 +143,12 @@
static const int kLiftingTimeout;
// Time that it takes to shoot the disc in cycles.
static const int kShootingDelay;
- // Time that it takes to lower the loader in cycles.
+ // Time that it takes to finish lowering the loader after the sensor is
+ // triggered in cycles.
static const int kLoweringDelay;
+ // Time until we give up lowering and move on in cycles.
+ // It's a long time because we really don't want to ever hit this.
+ static const int kLoweringTimeout;
// Object representing a Frisbee tracked by the indexer.
class Frisbee {
@@ -313,18 +317,24 @@
LoaderGoal loader_goal_;
LoaderState loader_state_;
int loader_countdown_, loader_timeout_;
+ // Whether or not we (might have) failed to shoot a disc that's now (probably)
+ // still in the loader.
+ bool disc_stuck_in_loader_;
// Current state of the pistons.
bool loader_up_;
bool disc_clamped_;
bool disc_ejected_;
+ bool is_shooting_;
+
// The frisbee that is flying through the transfer rollers.
Frisbee transfer_frisbee_;
// Bottom disc detect from the last valid packet for detecting edges.
bool last_bottom_disc_detect_;
bool last_top_disc_detect_;
+ bool hopper_clear_;
int32_t last_bottom_disc_posedge_count_;
int32_t last_bottom_disc_negedge_count_;
int32_t last_bottom_disc_negedge_wait_count_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index 3956b18..9530a4f 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -20,6 +20,8 @@
// If true, set the indexer voltage to index_voltage.
bool override_index;
double index_voltage;
+ bool override_transfer;
+ double transfer_voltage;
};
message Position {
@@ -45,8 +47,10 @@
int32_t top_disc_negedge_count;
double top_disc_negedge_position;
- // Whether the hall effect for the loader being at the top is triggered.
+ // Whether the hall effects for the loader are triggered (have a magnet in
+ // front of them).
bool loader_top;
+ bool loader_bottom;
};
message Output {
@@ -75,6 +79,12 @@
bool preloaded;
// Indexer ready to accept more discs.
bool ready_to_intake;
+ // True from when we're committed to shooting util after the disk is clear
+ // of the robot.
+ bool is_shooting;
+ // Goes false when we first get a disk and back true after we finish
+ // clearing.
+ bool hopper_clear;
};
queue Goal goal;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 981df5d..035880e 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -4,6 +4,7 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/index/index_motor.q.h"
namespace frc971 {
namespace control_loops {
@@ -13,7 +14,8 @@
loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
history_position_(0),
position_goal_(0.0),
- last_position_(0.0) {
+ last_position_(0.0),
+ last_velocity_goal_(0) {
memset(history_, 0, sizeof(history_));
}
@@ -26,11 +28,23 @@
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
- const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ double velocity_goal = std::min(goal->velocity, kMaxSpeed);
const double current_position =
(position == NULL ? loop_->X_hat(0, 0) : position->position);
double output_voltage = 0.0;
+ if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ &&
+ velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }
+ last_velocity_goal_ = velocity_goal;
+
// Track the current position if the velocity goal is small.
if (velocity_goal <= 1.0) {
position_goal_ = current_position;
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index b1371a4..41c33c1 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -29,6 +29,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:common',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
],
'export_dependent_settings': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 77c605b..7947f7a 100644
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -45,6 +45,9 @@
double position_goal_;
double last_position_;
+ // For making sure it keeps spinning if we're shooting.
+ double last_velocity_goal_;
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/crio/build.sh b/frc971/crio/build.sh
index a612478..b3fc031 100755
--- a/frc971/crio/build.sh
+++ b/frc971/crio/build.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-../../aos/build/build.sh crio crio.gyp no crio $1
+../../aos/build/build.sh crio crio.gyp no "$@"
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index 3650e96..fb47642 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -1,75 +1,11 @@
#ifndef FRC971_INPUT_GYRO_BOARD_DATA_H_
#define FRC971_INPUT_GYRO_BOARD_DATA_H_
-#include "aos/common/byteorder.h"
-
namespace frc971 {
-// The struct that the gyro board sends out with all of the data in it.
-struct GyroBoardData {
- int64_t gyro_angle;
-
- int32_t left_drive;
- int32_t right_drive;
- int32_t shooter_angle;
- int32_t shooter;
- int32_t indexer;
- int32_t wrist;
-
- int32_t capture_top_rise;
- int32_t capture_top_fall;
- int32_t capture_bottom_fall_delay;
- int32_t capture_wrist_rise;
- int32_t capture_shooter_angle_rise;
-
- uint8_t top_rise_count;
-
- uint8_t top_fall_count;
-
- uint8_t bottom_rise_count;
-
- uint8_t bottom_fall_delay_count;
- uint8_t bottom_fall_count;
-
- uint8_t wrist_rise_count;
-
- uint8_t shooter_angle_rise_count;
-
- union {
- struct {
- uint8_t wrist_hall_effect : 1;
- uint8_t angle_adjust_bottom_hall_effect : 1;
- uint8_t top_disc : 1;
- uint8_t bottom_disc : 1;
- uint8_t loader_top : 1;
- };
- uint32_t digitals;
- };
-
- void NetworkToHost() {
- // Apparently it sends the information out in little endian.
-#if 0
- using ::aos::ntoh;
-
- gyro_angle = ntoh(gyro_angle);
-
- right_drive = ntoh(right_drive);
- left_drive = ntoh(left_drive);
- shooter_angle = ntoh(shooter_angle);
- shooter = ntoh(shooter);
- indexer = ntoh(indexer);
- wrist = ntoh(wrist);
-
- capture_top_rise = ntoh(capture_top_rise);
- capture_top_fall = ntoh(capture_top_fall);
- capture_bottom_fall_delay = ntoh(capture_bottom_fall_delay);
- capture_wrist_rise = ntoh(capture_wrist_rise);
- capture_shooter_angle_rise = ntoh(capture_shooter_angle_rise);
-
- digitals = ntoh(digitals);
-#endif
- }
-} __attribute__((__packed__));
+#define DATA_STRUCT_NAME GyroBoardData
+#include "../../gyro_board/src/usb/data_struct.h"
+#undef DATA_STRUCT_NAME
} // namespace frc971
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
deleted file mode 100644
index 817efa9..0000000
--- a/frc971/input/gyro_board_reader.cc
+++ /dev/null
@@ -1,268 +0,0 @@
-#include <libusb-1.0/libusb.h>
-#include <memory>
-
-#include "aos/common/inttypes.h"
-#include "aos/atom_code/init.h"
-#include "aos/common/logging/logging.h"
-#include "aos/common/control_loop/Timing.h"
-#include "aos/common/time.h"
-
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "frc971/control_loops/wrist/wrist_motor.q.h"
-#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
-#include "frc971/control_loops/index/index_motor.q.h"
-#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/input/gyro_board_data.h"
-#include "gyro_board/src/libusb-driver/libusb_wrap.h"
-#include "frc971/queues/GyroAngle.q.h"
-#include "frc971/constants.h"
-#include "aos/common/messages/RobotState.q.h"
-
-#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
-
-using ::frc971::control_loops::drivetrain;
-using ::frc971::control_loops::wrist;
-using ::frc971::control_loops::angle_adjust;
-using ::frc971::control_loops::shooter;
-using ::frc971::control_loops::index_loop;
-using ::frc971::sensors::gyro;
-
-namespace frc971 {
-namespace {
-
-inline double drivetrain_translate(int32_t in) {
- int pinion_size;
- if (::aos::robot_state.get() == NULL) {
- if (!::aos::robot_state.FetchNext()) {
- LOG(WARNING, "couldn't fetch robot state\n");
- return 0;
- }
- }
- if (!constants::drivetrain_gearbox_pinion(&pinion_size)) return 0;
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (pinion_size / 50.0) /*output reduction*/ *
- (64.0 / 24.0) /*encoder gears*/ *
- (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
-}
-
-inline double wrist_translate(int32_t in) {
- return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (14.0 / 50.0 * 20.0 / 84.0) /*gears*/ * (2 * M_PI);
-}
-
-inline double angle_adjust_translate(int32_t in) {
- static const double kCableDiameter = 0.060;
- return -static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- ((0.75 + kCableDiameter) / (16.61125 + kCableDiameter)) /*pulleys*/ *
- (2 * M_PI);
-}
-
-inline double shooter_translate(int32_t in) {
- return static_cast<double>(in) / (32.0 /*cpr*/ * 4.0 /*quad*/) *
- (15.0 / 34.0) /*gears*/ * (2 * M_PI);
-}
-
-inline double index_translate(int32_t in) {
- return -static_cast<double>(in) / (128.0 /*cpr*/ * 4.0 /*quad*/) *
- (1.0) /*gears*/ * (2 * M_PI);
-}
-
-} // namespace
-
-class GyroBoardReader {
- public:
- GyroBoardReader()
- : top_rise_count_(0),
- last_top_rise_count_(0),
- top_fall_count_(0),
- last_top_fall_count_(0),
- bottom_rise_count_(0),
- last_bottom_rise_count_(0),
- bottom_fall_delay_count_(0),
- last_bottom_fall_delay_count_(0),
- bottom_fall_count_(0),
- last_bottom_fall_count_(0),
- wrist_rise_count_(0),
- last_wrist_rise_count_(0),
- shooter_angle_rise_count_(0),
- last_shooter_angle_rise_count_(0) {
- }
-
- void Run() {
- LibUSB libusb;
-
- dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
- libusb.FindDeviceWithVIDPID(kVid, kPid));
- if (!dev_handle_) {
- LOG(ERROR, "couldn't find device. exiting\n");
- exit(1);
- }
-
- uint8_t data[64];
- GyroBoardData *real_data;
- static_assert(sizeof(*real_data) <= sizeof(data), "it doesn't fit");
-
- uint8_t *data_pointer = data;
- memcpy(&real_data, &data_pointer, sizeof(data_pointer));
- while (true) {
- if (false) {
- // Theoretically need -3ms of offset. Using a slightly larger one to avoid
- // missing the first control loop in the worst case.
- ::aos::time::PhasedLoop10MS(
- ::aos::time::Time::InSeconds(-0.0031).ToUSec());
- LOG(DEBUG, "starting now\n");
-
- // Read 2 to make sure that we get fresh data.
- if (!ReadPacket(data, sizeof(data))) continue;
- //LOG(DEBUG, "in between\n");
- if (!ReadPacket(data, sizeof(data))) continue;
- } else {
- if (!ReadPacket(data, sizeof(data))) continue;
-
- ProcessData(real_data);
- }
- }
- }
-
- private:
- static const unsigned char kEndpoint = 0x81;
- // in ms
- // 0 is unlimited
- static const unsigned int kReadTimeout = 1000;
-
- // vendor ID
- static const int32_t kVid = 0x1424;
- // product ID
- static const int32_t kPid = 0xd243;
-
- // Returns whether it read a good packet.
- bool ReadPacket(uint8_t *data, size_t data_size) {
- int read_bytes;
- int r = dev_handle_->interrupt_transfer(
- kEndpoint, data, data_size, &read_bytes, kReadTimeout);
-
- if (r != 0) {
- if (r == LIBUSB_ERROR_TIMEOUT) {
- LOG(ERROR, "read timed out\n");
- return false;
- }
- LOG(FATAL, "libusb gave error %d\n", r);
- }
-
- if (read_bytes < static_cast<ssize_t>(sizeof(GyroBoardData))) {
- LOG(ERROR, "read %d bytes instead of at least %zd\n",
- read_bytes, sizeof(GyroBoardData));
- return false;
- }
-
- return true;
- }
-
- void UpdateWrappingCounter(
- uint8_t current, uint8_t *last, int32_t *counter) {
- if (*last > current) {
- *counter += 0x100;
- }
- *counter = (*counter & 0xffffff00) | current;
- *last = current;
- }
-
- void ProcessData(GyroBoardData *data) {
- data->NetworkToHost();
- LOG(DEBUG, "processing a packet\n");
- static ::aos::time::Time last_time = ::aos::time::Time::Now();
- if ((last_time - ::aos::time::Time::Now()) >
- ::aos::time::Time::InMS(0.00205)) {
- LOG(INFO, "missed one\n");
- }
-
- gyro.MakeWithBuilder()
- .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
- .Send();
-
- UpdateWrappingCounter(data->top_rise_count,
- &last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->top_fall_count,
- &last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->bottom_rise_count,
- &last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->bottom_fall_delay_count,
- &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->bottom_fall_count,
- &last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
- &last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->shooter_angle_rise_count,
- &last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
-
- drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
- .Send();
-
- wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
- .Send();
-
- angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(data->angle_adjust_bottom_hall_effect)
- .middle_hall_effect(false)
- .bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
- .middle_calibration(angle_adjust_translate(
- 0))
- .Send();
-
- shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
- .Send();
-
- index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(data->top_disc)
- .top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
- .top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(data->bottom_disc)
- .bottom_disc_posedge_count(bottom_rise_count_)
- .bottom_disc_negedge_count(bottom_fall_count_)
- .bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
- .loader_top(data->loader_top)
- .Send();
- }
-
- ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
-
- int32_t top_rise_count_;
- uint8_t last_top_rise_count_;
- int32_t top_fall_count_;
- uint8_t last_top_fall_count_;
- int32_t bottom_rise_count_;
- uint8_t last_bottom_rise_count_;
- int32_t bottom_fall_delay_count_;
- uint8_t last_bottom_fall_delay_count_;
- int32_t bottom_fall_count_;
- uint8_t last_bottom_fall_count_;
- int32_t wrist_rise_count_;
- uint8_t last_wrist_rise_count_;
- int32_t shooter_angle_rise_count_;
- uint8_t last_shooter_angle_rise_count_;
-};
-
-} // namespace frc971
-
-int main() {
- ::aos::Init();
- ::frc971::GyroBoardReader reader;
- reader.Run();
- ::aos::Cleanup();
- return 0;
-}
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 4dec23e..42b50c8 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -1,21 +1,14 @@
-#include <libusb-1.0/libusb.h>
-#include <memory>
-
-#include "aos/common/inttypes.h"
#include "aos/atom_code/init.h"
#include "aos/common/logging/logging.h"
-#include "aos/common/time.h"
-#include "aos/common/sensors/sensor_unpacker.h"
-#include "aos/common/sensors/sensor_receiver.h"
+#include "aos/common/util/wrapping_counter.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/wrist/wrist_motor.q.h"
#include "frc971/control_loops/angle_adjust/angle_adjust_motor.q.h"
#include "frc971/control_loops/index/index_motor.q.h"
#include "frc971/control_loops/shooter/shooter_motor.q.h"
-#include "frc971/input/gyro_board_data.h"
-#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/input/usb_receiver.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -27,6 +20,7 @@
using ::frc971::control_loops::shooter;
using ::frc971::control_loops::index_loop;
using ::frc971::sensors::gyro;
+using ::aos::util::WrappingCounter;
namespace frc971 {
namespace {
@@ -61,238 +55,82 @@
} // namespace
-class GyroSensorUnpacker :
- public ::aos::sensors::SensorUnpackerInterface<GyroBoardData> {
- public:
- GyroSensorUnpacker()
- : top_rise_count_(0),
- last_top_rise_count_(0),
- top_fall_count_(0),
- last_top_fall_count_(0),
- bottom_rise_count_(0),
- last_bottom_rise_count_(0),
- bottom_fall_delay_count_(0),
- last_bottom_fall_delay_count_(0),
- bottom_fall_count_(0),
- last_bottom_fall_count_(0),
- wrist_rise_count_(0),
- last_wrist_rise_count_(0),
- shooter_angle_rise_count_(0),
- last_shooter_angle_rise_count_(0) {
- }
-
- void UnpackFrom(GyroBoardData *data) {
- data->NetworkToHost();
- LOG(DEBUG, "processing a packet\n");
-
- static ::aos::time::Time last_time = ::aos::time::Time::Now();
- if ((last_time - ::aos::time::Time::Now()) >
- ::aos::time::Time::InMS(0.00205)) {
- LOG(INFO, "missed one\n");
+class GyroSensorReceiver : public USBReceiver {
+ virtual void ProcessData() override {
+ if (data()->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n",
+ data()->robot_id, data()->base_status & 0xF);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n",
+ data()->base_status & 0xF);
}
gyro.MakeWithBuilder()
- .angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
+ .angle(data()->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- UpdateWrappingCounter(data->top_rise_count,
- &last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->top_fall_count,
- &last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->bottom_rise_count,
- &last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->bottom_fall_delay_count,
- &last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->bottom_fall_count,
- &last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
- &last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->shooter_angle_rise_count,
- &last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
-
drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
+ .right_encoder(drivetrain_translate(data()->main.right_drive))
+ .left_encoder(-drivetrain_translate(data()->main.left_drive))
.Send();
wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(!data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
+ .pos(wrist_translate(data()->main.wrist))
+ .hall_effect(data()->main.wrist_hall_effect)
+ .calibration(wrist_translate(data()->main.capture_wrist_rise))
.Send();
angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(!data->angle_adjust_bottom_hall_effect)
+ .angle(angle_adjust_translate(data()->main.shooter_angle))
+ .bottom_hall_effect(data()->main.angle_adjust_bottom_hall_effect)
.middle_hall_effect(false)
.bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
+ data()->main.capture_shooter_angle_rise))
.middle_calibration(angle_adjust_translate(
0))
.Send();
shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
+ .position(shooter_translate(data()->main.shooter))
.Send();
index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(!data->top_disc)
- .top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
- .top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(!data->bottom_disc)
- .bottom_disc_posedge_count(bottom_rise_count_)
- .bottom_disc_negedge_count(bottom_fall_count_)
+ .index_position(index_translate(data()->main.indexer))
+ .top_disc_detect(data()->main.top_disc)
+ .top_disc_posedge_count(top_rise_.Update(data()->main.top_rise_count))
+ .top_disc_posedge_position(
+ index_translate(data()->main.capture_top_rise))
+ .top_disc_negedge_count(top_fall_.Update(data()->main.top_fall_count))
+ .top_disc_negedge_position(
+ index_translate(data()->main.capture_top_fall))
+ .bottom_disc_detect(data()->main.bottom_disc)
+ .bottom_disc_posedge_count(
+ bottom_rise_.Update(data()->main.bottom_rise_count))
+ .bottom_disc_negedge_count(
+ bottom_fall_.Update(data()->main.bottom_fall_count))
.bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
- .bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ data()->main.capture_bottom_fall_delay))
+ .bottom_disc_negedge_wait_count(
+ bottom_fall_delay_.Update(data()->main.bottom_fall_delay_count))
+ .loader_top(data()->main.loader_top)
+ .loader_bottom(data()->main.loader_bottom)
.Send();
}
- private:
- void UpdateWrappingCounter(
- uint8_t current, uint8_t *last, int32_t *counter) {
- if (*last > current) {
- *counter += 0x100;
- }
- *counter = (*counter & 0xffffff00) | current;
- *last = current;
- }
-
- int32_t top_rise_count_;
- uint8_t last_top_rise_count_;
- int32_t top_fall_count_;
- uint8_t last_top_fall_count_;
- int32_t bottom_rise_count_;
- uint8_t last_bottom_rise_count_;
- int32_t bottom_fall_delay_count_;
- uint8_t last_bottom_fall_delay_count_;
- int32_t bottom_fall_count_;
- uint8_t last_bottom_fall_count_;
- int32_t wrist_rise_count_;
- uint8_t last_wrist_rise_count_;
- int32_t shooter_angle_rise_count_;
- uint8_t last_shooter_angle_rise_count_;
-};
-
-class GyroSensorReceiver :
- public ::aos::sensors::SensorReceiver<GyroBoardData> {
- public:
- GyroSensorReceiver(
- ::aos::sensors::SensorUnpackerInterface<GyroBoardData> *unpacker)
- : ::aos::sensors::SensorReceiver<GyroBoardData>(unpacker),
- start_time_(0, 0) {
- static_assert(sizeof(GyroBoardData) <= kDataLength,
- "the buffer will be too small");
- }
-
- private:
- static const unsigned char kEndpoint = 0x81;
- // in ms
- // 0 is unlimited
- static const unsigned int kReadTimeout = 1000;
-
- // vendor ID
- static const int32_t kVid = 0x1424;
- // product ID
- static const int32_t kPid = 0xd243;
-
- // How big of a buffer to give the function.
- static const size_t kDataLength = 64;
-
- virtual void DoReceiveData() {
- // Loop and then return once we get a good one.
- while (true) {
- completed_transfer_ = NULL;
- while (completed_transfer_ == NULL) {
- libusb_.HandleEvents();
- }
- LOG(DEBUG, "processing transfer %p\n", completed_transfer_);
-
- if (completed_transfer_->read_bytes() <
- static_cast<ssize_t>(sizeof(GyroBoardData))) {
- LOG(ERROR, "read %d bytes instead of at least %zd\n",
- completed_transfer_->read_bytes(), sizeof(GyroBoardData));
- continue;
- }
-
- memcpy(&data()->values, completed_transfer_->data(),
- sizeof(GyroBoardData));
- if (data()->count == 0) {
- start_time_ = ::aos::time::Time::Now();
- data()->count = 1;
- } else {
- ::aos::time::Time delta_time = ::aos::time::Time::Now() - start_time_;
- data()->count = static_cast<int32_t>(
- (delta_time / ::aos::sensors::kSensorSendFrequency) + 0.5);
- }
- return;
- }
- }
-
- virtual void Reset() {
- transfer1_.reset();
- transfer2_.reset();
- dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
- libusb_.FindDeviceWithVIDPID(kVid, kPid));
- if (!dev_handle_) {
- LOG(ERROR, "couldn't find device. exiting\n");
- exit(1);
- }
- transfer1_ = ::std::unique_ptr<libusb::Transfer>(
- new libusb::Transfer(kDataLength, StaticTransferCallback, this));
- transfer2_ = ::std::unique_ptr<libusb::Transfer>(
- new libusb::Transfer(kDataLength, StaticTransferCallback, this));
- transfer1_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
- transfer2_->FillInterrupt(dev_handle_.get(), kEndpoint, kReadTimeout);
- transfer1_->Submit();
- transfer2_->Submit();
-
- data()->count = 0;
- }
-
- static void StaticTransferCallback(libusb::Transfer *transfer, void *self) {
- static_cast<GyroSensorReceiver *>(self)->TransferCallback(transfer);
- }
- void TransferCallback(libusb::Transfer *transfer) {
- if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
- LOG(DEBUG, "transfer %p completed\n", transfer);
- completed_transfer_ = transfer;
- } else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
- LOG(WARNING, "transfer %p timed out\n", transfer);
- } else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
- LOG(DEBUG, "transfer %p cancelled\n", transfer);
- } else {
- LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
- }
- transfer->Submit();
- }
-
- virtual void Synchronized(::aos::time::Time start_time) {
- // Subtract off how many packets it read while synchronizing from the time.
- start_time_ = start_time -
- ::aos::sensors::kSensorSendFrequency * data()->count;
- }
-
- ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
- ::std::unique_ptr<libusb::Transfer> transfer1_, transfer2_;
- // Temporary variable for holding a completed transfer to communicate that
- // information from the callback to the code that wants it.
- libusb::Transfer *completed_transfer_;
-
- ::aos::time::Time start_time_;
-
- LibUSB libusb_;
+ WrappingCounter top_rise_;
+ WrappingCounter top_fall_;
+ WrappingCounter bottom_rise_;
+ WrappingCounter bottom_fall_delay_;
+ WrappingCounter bottom_fall_;
};
} // namespace frc971
int main() {
::aos::Init();
- ::frc971::GyroSensorUnpacker unpacker;
- ::frc971::GyroSensorReceiver receiver(&unpacker);
+ ::frc971::GyroSensorReceiver receiver;
while (true) {
receiver.RunIteration();
}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 05354ef..d3e25a1 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -35,46 +35,28 @@
'<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
'<(AOS)/atom_code/atom_code.gyp:init',
- '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(EXTERNALS):libusb',
'<(AOS)/build/aos.gyp:logging',
- '<(AOS)/common/common.gyp:time',
- '<(AOS)/common/sensors/sensors.gyp:sensor_receiver',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ 'usb_receiver',
],
},
{
- 'target_name': 'gyro_board_reader',
- 'type': 'executable',
+ 'target_name': 'usb_receiver',
+ 'type': 'static_library',
'sources': [
- 'gyro_board_reader.cc',
+ 'usb_receiver.cc',
],
'dependencies': [
- '<(DEPTH)/frc971/control_loops/drivetrain/drivetrain.gyp:drivetrain_loop',
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(DEPTH)/frc971/control_loops/angle_adjust/angle_adjust.gyp:angle_adjust_loop',
- '<(DEPTH)/frc971/control_loops/wrist/wrist.gyp:wrist_loop',
- '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
- '<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
- '<(AOS)/atom_code/atom_code.gyp:init',
'<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
- '<(EXTERNALS):libusb',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
'<(AOS)/common/common.gyp:time',
- '<(AOS)/common/common.gyp:timing',
- '<(DEPTH)/frc971/frc971.gyp:common',
- '<(AOS)/common/messages/messages.gyp:aos_queues',
+ '<(AOS)/common/common.gyp:controls',
],
- },
- {
- 'target_name': 'gyro_reader',
- 'type': 'executable',
- 'sources': [
- 'gyro_reader.cc',
- ],
- 'dependencies': [
- '<(DEPTH)/frc971/queues/queues.gyp:queues',
- '<(AOS)/atom_code/atom_code.gyp:init',
- '<(AOS)/build/aos.gyp:logging',
+ 'export_dependent_settings': [
+ '<(DEPTH)/gyro_board/src/libusb-driver/libusb-driver.gyp:libusb_wrap',
+ '<(AOS)/common/util/util.gyp:wrapping_counter',
+ '<(AOS)/common/common.gyp:time',
],
},
],
diff --git a/frc971/input/joystick_reader.cc b/frc971/input/joystick_reader.cc
index 3943350..be4e188 100644
--- a/frc971/input/joystick_reader.cc
+++ b/frc971/input/joystick_reader.cc
@@ -52,6 +52,7 @@
const ButtonLocation kIntake(3, 10);
const ButtonLocation kForceFire(3, 12);
const ButtonLocation kForceIndexUp(3, 9), kForceIndexDown(3, 7);
+const ButtonLocation kForceSpitOut(2, 11);
const ButtonLocation kDeployHangers(3, 1);
@@ -65,6 +66,7 @@
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
static bool is_high_gear = false;
+ static double angle_adjust_goal = 0.42;
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
@@ -136,6 +138,10 @@
is_high_gear = true;
}
+ // Whether we should change wrist positions to indicate that the hopper is
+ // clear.
+ bool hopper_clear = false;
+
// Where the wrist should be to pick up a frisbee.
// TODO(brians): Make these globally accessible and clean up auto.
static const double kWristPickup = -0.580;
@@ -143,15 +149,26 @@
// Where the wrist gets stored when up.
// All the way up is 1.5.
static const double kWristUp = 1.43;
+ static const double kWristCleared = kWristUp - 0.2;
static double wrist_down_position = kWristPickup;
double wrist_up_position = kWristUp;
+ double wrist_pickup_position = data.IsPressed(kIntake) ?
+ kWristPickup : kWristNearGround;
+ if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->hopper_disc_count >= 4) {
+ wrist_down_position = kWristNearGround;
+ } else {
+ wrist_down_position = wrist_pickup_position;
+ }
+ hopper_clear = index_loop.status->hopper_clear;
+ }
::aos::ScopedMessagePtr<control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
shooter_goal->velocity = 0;
- static double angle_adjust_goal = 0.42;
if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
shooter_goal->velocity = 131;
+ if (hopper_clear) wrist_up_position = kWristCleared;
angle_adjust_goal = 0.70;
} else if (data.IsPressed(kLongShot)) {
#if 0
@@ -166,9 +183,10 @@
// pretend like no button is pressed
}
#endif
+ // This shot is from 30'.
shooter_goal->velocity = 360;
- wrist_up_position = 1.23 - 0.4;
- angle_adjust_goal = 0.596;
+ if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
+ angle_adjust_goal = 0.630;
} else if (data.IsPressed(kMediumShot)) {
#if 0
shooter_goal->velocity = 375;
@@ -177,24 +195,15 @@
#endif
// middle wheel on the back line (same as auto)
shooter_goal->velocity = 395;
- wrist_up_position = 1.23 - 0.4;
+ if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.520;
} else if (data.IsPressed(kShortShot)) {
shooter_goal->velocity = 375;
+ if (hopper_clear) wrist_up_position = kWristCleared;
angle_adjust_goal = 0.671;
}
angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
- double wrist_pickup_position = data.IsPressed(kIntake) ?
- kWristPickup : kWristNearGround;
- index_loop.status.FetchLatest();
- if (index_loop.status.get()) {
- if (index_loop.status->hopper_disc_count >= 4) {
- wrist_down_position = kWristNearGround;
- } else {
- wrist_down_position = wrist_pickup_position;
- }
- }
wrist.goal.MakeWithBuilder()
.goal(data.IsPressed(kWristDown) ?
wrist_down_position :
@@ -220,7 +229,9 @@
const bool index_up = data.IsPressed(kForceIndexUp);
const bool index_down = data.IsPressed(kForceIndexDown);
- index_goal->override_index = index_up || index_down;
+ const bool spit_out = data.IsPressed(kForceSpitOut);
+ index_goal->override_index = index_up || index_down || spit_out;
+ index_goal->override_transfer = spit_out;
if (index_up && index_down) {
index_goal->index_voltage = 0.0;
} else if (index_up) {
@@ -228,6 +239,10 @@
} else if (index_down) {
index_goal->index_voltage = -12.0;
}
+ if (spit_out) {
+ index_goal->index_voltage = -12.0;
+ index_goal->transfer_voltage = -12.0;
+ }
index_goal.Send();
shooter_goal.Send();
@@ -236,6 +251,7 @@
static int hanger_cycles = 0;
if (data.IsPressed(kDeployHangers)) {
++hanger_cycles;
+ angle_adjust_goal = 0.4;
} else {
hanger_cycles = 0;
}
diff --git a/frc971/input/usb_receiver.cc b/frc971/input/usb_receiver.cc
new file mode 100644
index 0000000..545e197
--- /dev/null
+++ b/frc971/input/usb_receiver.cc
@@ -0,0 +1,236 @@
+#include <string.h>
+#include <errno.h>
+#include <inttypes.h>
+
+#include "frc971/input/usb_receiver.h"
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/control_loop/ControlLoop.h"
+
+namespace frc971 {
+
+USBReceiver::USBReceiver() {
+ Reset();
+}
+
+void USBReceiver::RunIteration() {
+ if (ReceiveData()) {
+ Reset();
+ } else {
+ const ::aos::time::Time received_time = ::aos::time::Time::Now();
+ if (phase_locker_.IsCurrentPacketGood(received_time, sequence_.count())) {
+ LOG(DEBUG, "processing data\n");
+ ProcessData();
+ }
+ }
+}
+
+void USBReceiver::PhaseLocker::Reset() {
+ LOG(INFO, "resetting\n");
+ last_good_packet_time_ = ::aos::time::Time(0, 0);
+ last_good_sequence_ = -1;
+ good_phase_ = guess_phase_ = kUnknownPhase;
+ guess_phase_good_ = guess_phase_bad_ = 0;
+ good_phase_early_ = good_phase_late_ = 0;
+}
+
+bool USBReceiver::PhaseLocker::IsCurrentPacketGood(
+ const ::aos::time::Time &received_time,
+ int32_t sequence) {
+ if (last_good_packet_time_ != ::aos::time::Time(0, 0) &&
+ received_time - last_good_packet_time_ > kResetTime) {
+ LOG(WARNING, "no good packet received in too long\n");
+ Reset();
+ return false;
+ }
+ if (last_good_sequence_ != -1 && sequence - last_good_sequence_ > 100) {
+ LOG(WARNING, "skipped too many packets\n");
+ Reset();
+ return false;
+ }
+
+ using ::aos::control_loops::kLoopFrequency;
+ // How often we (should) receive packets.
+ static const ::aos::time::Time kPacketFrequency =
+ kLoopFrequency / kPacketsPerLoopCycle;
+ static const ::aos::time::Time kPacketClose =
+ kPacketFrequency * 65 / 100;
+ static const ::aos::time::Time kSwitchOffset =
+ kPacketFrequency * 6 / 10;
+
+ // When we want to receive a packet for the next cycle of control loops.
+ ::aos::time::Time next_desired =
+ ::aos::control_loops::NextLoopTime(received_time) + kDesiredOffset;
+ // If we came up with something more than 1 packet in the past.
+ if (next_desired - received_time < -kPacketFrequency) {
+ next_desired += kLoopFrequency;
+ }
+ // How far off of when we want the next packet this one is.
+ const ::aos::time::Time offset = next_desired - received_time;
+
+ const int received_phase = sequence % kPacketsPerLoopCycle;
+
+ assert(!(good_phase_early_ != 0 && good_phase_late_ != 0));
+
+ if (good_phase_ == kUnknownPhase &&
+ guess_phase_good_ > kMinGoodGuessCycles) {
+ good_phase_ = guess_phase_;
+ if (guess_phase_offset_ < kPacketFrequency / -2) {
+ ++good_phase_;
+ } else if (guess_phase_offset_ > kPacketFrequency / 2) {
+ --good_phase_;
+ }
+ LOG(INFO, "locked on to phase %d\n", good_phase_);
+ } else if (guess_phase_bad_ > kMaxBadGuessCycles) {
+ LOG(INFO, "guessed wrong phase too many times\n");
+ Reset();
+ }
+ if (good_phase_early_ > kSwitchCycles) {
+ good_phase_early_ = 0;
+ LOG(INFO, "switching from phase %d to %d-1\n",
+ good_phase_, good_phase_);
+ --good_phase_;
+ } else if (good_phase_late_ > kSwitchCycles) {
+ good_phase_late_ = 0;
+ LOG(INFO, "switching from phase %d to %d+1\n",
+ good_phase_, good_phase_);
+ ++good_phase_;
+ }
+ if (good_phase_ == kUnknownPhase) {
+ LOG(DEBUG, "guessing which packet is good\n");
+
+ // If it's close to the right time.
+ if (offset.abs() < kPacketClose) {
+ if (guess_phase_ == kUnknownPhase) {
+ if (offset.abs() < kPacketFrequency * 55 / 100) {
+ guess_phase_ = received_phase;
+ guess_phase_offset_ = offset;
+ }
+ } else if (received_phase == guess_phase_) {
+ LOG(DEBUG, "guessed right phase %d\n", received_phase);
+ ++guess_phase_good_;
+ guess_phase_bad_ = 0;
+ guess_phase_offset_ = (guess_phase_offset_ * 9 + offset) / 10;
+ }
+ } else if (guess_phase_ != kUnknownPhase &&
+ received_phase == guess_phase_) {
+ LOG(DEBUG, "guessed wrong phase %d\n", received_phase);
+ ++guess_phase_bad_;
+ guess_phase_good_ = ::std::max(0, guess_phase_good_ -
+ (kMinGoodGuessCycles / 10));
+ }
+ return false;
+ } else { // we know what phase we're looking for
+ // Deal with it if the above logic for tweaking the phase that we're
+ // using wrapped it around.
+ if (good_phase_ == -1) {
+ good_phase_ = kPacketsPerLoopCycle;
+ } else if (good_phase_ == kPacketsPerLoopCycle) {
+ LOG(DEBUG, "dewrapping\n");
+ good_phase_ = 0;
+ }
+ assert(good_phase_ >= 0);
+ assert(good_phase_ < kPacketsPerLoopCycle);
+
+ if (received_phase == good_phase_) {
+ if (offset < -kSwitchOffset) {
+ ++good_phase_early_;
+ good_phase_late_ = 0;
+ } else if (offset > kSwitchOffset) {
+ ++good_phase_late_;
+ good_phase_early_ = 0;
+ } else {
+ good_phase_early_ = good_phase_late_ = 0;
+ }
+ last_good_packet_time_ = received_time;
+ last_good_sequence_ = sequence;
+
+ return true;
+ } else {
+ return false;
+ }
+ }
+}
+
+void USBReceiver::StaticTransferCallback(libusb::Transfer *transfer,
+ void *self) {
+ static_cast<USBReceiver *>(self)->TransferCallback(transfer);
+}
+
+void USBReceiver::TransferCallback(libusb::Transfer *transfer) {
+ if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
+ LOG(DEBUG, "transfer %p completed\n", transfer);
+ completed_transfer_ = transfer;
+ } else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
+ LOG(WARNING, "transfer %p timed out\n", transfer);
+ completed_transfer_ = kTransferFailed;
+ } else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
+ LOG(DEBUG, "transfer %p cancelled\n", transfer);
+ } else {
+ LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
+ }
+ transfer->Submit();
+}
+
+bool USBReceiver::ReceiveData() {
+ // Loop and then return once we get a good one.
+ while (true) {
+ completed_transfer_ = NULL;
+ while (completed_transfer_ == NULL) {
+ libusb_.HandleEvents();
+ }
+ if (completed_transfer_ == kTransferFailed) {
+ LOG(WARNING, "transfer failed\n");
+ return true;
+ }
+
+ if (completed_transfer_->read_bytes() <
+ static_cast<ssize_t>(sizeof(GyroBoardData))) {
+ LOG(ERROR, "read %d bytes instead of at least %zd\n",
+ completed_transfer_->read_bytes(), sizeof(GyroBoardData));
+ continue;
+ }
+
+ memcpy(data(), completed_transfer_->data(),
+ sizeof(GyroBoardData));
+
+ int32_t count_before = sequence_.count();
+ sequence_.Update(data()->sequence);
+ if (count_before == 0) {
+ LOG(INFO, "count starting at %" PRId32 "\n", sequence_.count());
+ } else if (sequence_.count() - count_before != 1) {
+ LOG(WARNING, "count went from %" PRId32" to %" PRId32 "\n",
+ count_before, sequence_.count());
+ }
+
+ return false;
+ }
+}
+
+void USBReceiver::Reset() {
+ typedef ::std::unique_ptr<libusb::IsochronousTransfer> TransferType;
+ for (TransferType &c : transfers_) {
+ c.reset();
+ }
+ dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
+ libusb_.FindDeviceWithVIDPID(kVid, kPid));
+ if (!dev_handle_) {
+ LOG(ERROR, "couldn't find device. exiting\n");
+ exit(1);
+ }
+ for (TransferType &c : transfers_) {
+ c.reset(new libusb::IsochronousTransfer(kDataLength, 1,
+ StaticTransferCallback, this));
+ c->FillIsochronous(dev_handle_.get(), kEndpoint, kReadTimeout);
+ c->Submit();
+ }
+
+ sequence_.Reset();
+ phase_locker_.Reset();
+}
+
+constexpr ::aos::time::Time USBReceiver::kReadTimeout;
+constexpr ::aos::time::Time USBReceiver::kDesiredOffset;
+constexpr ::aos::time::Time USBReceiver::kResetTime;
+
+} // namespace frc971
diff --git a/frc971/input/usb_receiver.h b/frc971/input/usb_receiver.h
new file mode 100644
index 0000000..cda7843
--- /dev/null
+++ b/frc971/input/usb_receiver.h
@@ -0,0 +1,117 @@
+#ifndef FRC971_INPUT_USB_RECEIVER_H_
+#define FRC971_INPUT_USB_RECEIVER_H_
+
+#include <memory>
+
+#include "aos/common/time.h"
+#include "aos/common/util/wrapping_counter.h"
+
+#include "gyro_board/src/libusb-driver/libusb_wrap.h"
+#include "frc971/input/gyro_board_data.h"
+
+namespace frc971 {
+
+// TODO(brians): Figure out how to deal with the kernel bunching packets up on
+// us.
+class USBReceiver {
+ public:
+ USBReceiver();
+
+ void RunIteration();
+
+ protected:
+ GyroBoardData *data() { return &data_; }
+
+ private:
+ static const unsigned char kEndpoint = 0x83;
+ // 0 is unlimited
+ static constexpr ::aos::time::Time kReadTimeout =
+ ::aos::time::Time::InSeconds(1.5);
+ // vendor ID
+ static const int32_t kVid = 0x1424;
+ // product ID
+ static const int32_t kPid = 0xd243;
+
+ // A value to put into completed_transfer_ to indicate that it failed.
+ static constexpr libusb::Transfer *kTransferFailed =
+ reinterpret_cast<libusb::Transfer *>(-1);
+ // The kernel on the fitpc seems to miss ~11-15 packets in a row if it misses
+ // any with just 2, so 25 should be enough to ride over any holes.
+ static const int kNumTransfers = 25;
+
+ // How big of a buffer we're going to give the usb transfer stuff.
+ static const size_t kDataLength = 128;
+ static_assert(kDataLength >= sizeof(GyroBoardData), "buffer is too small");
+
+ static const int kPacketsPerLoopCycle = 10;
+
+ // How long "after" the control loops run we want to use a packet.
+ static constexpr ::aos::time::Time kDesiredOffset =
+ ::aos::time::Time::InSeconds(-0.003);
+
+ // How long without a good packet until we give up and Reset().
+ static constexpr ::aos::time::Time kResetTime =
+ ::aos::time::Time::InSeconds(0.25);
+
+ // Contains all of the complicated state and logic for locking onto the the
+ // correct phase.
+ class PhaseLocker {
+ public:
+ void Reset();
+
+ // Gets called for every packet received.
+ // Returns whether or not to process the values from this packet.
+ bool IsCurrentPacketGood(const ::aos::time::Time &received_time,
+ int32_t sequence);
+
+ private:
+ // How many times the packet we guessed has to be close to right to use the
+ // guess.
+ static const int kMinGoodGuessCycles = 30;
+ // How many times in a row we have to guess the wrong packet before trying
+ // again.
+ static const int kMaxBadGuessCycles = 3;
+
+ // How many times in a row a different packet has to be better than the one
+ // that we're using befor switching to it.
+ static const int kSwitchCycles = 15;
+
+ ::aos::time::Time last_good_packet_time_{0, 0};
+
+ int32_t last_good_sequence_;
+
+ const int kUnknownPhase = -11;
+ // kUnknownPhase or the sequence number (%kPacketsPerLoopCycle) to
+ // use or think about using.
+ // If not kUnknownPhase, 0 <= these < kPacketsPerLoopCycle.
+ int good_phase_, guess_phase_;
+ int guess_phase_good_, guess_phase_bad_;
+ ::aos::time::Time guess_phase_offset_{0, 0};
+ int good_phase_early_, good_phase_late_;
+ } phase_locker_;
+
+ static void StaticTransferCallback(libusb::Transfer *transfer, void *self);
+ void TransferCallback(libusb::Transfer *transfer);
+
+ // Returns true if receiving failed and we should try a Reset().
+ bool ReceiveData();
+
+ void Reset();
+
+ virtual void ProcessData() = 0;
+
+ GyroBoardData data_;
+
+ ::aos::util::WrappingCounter sequence_;
+
+ LibUSB libusb_;
+ ::std::unique_ptr<LibUSBDeviceHandle> dev_handle_;
+ ::std::unique_ptr<libusb::IsochronousTransfer> transfers_[kNumTransfers];
+ // "Temporary" variable for holding a completed transfer to communicate that
+ // information from the callback to the code that wants it.
+ libusb::Transfer *completed_transfer_;
+};
+
+} // namespace frc971
+
+#endif // FRC971_INPUT_USB_RECEIVER_H_
diff --git a/gyro_board/src/libusb-driver/libusb-driver.gyp b/gyro_board/src/libusb-driver/libusb-driver.gyp
index 1c50633..ab2bf70 100644
--- a/gyro_board/src/libusb-driver/libusb-driver.gyp
+++ b/gyro_board/src/libusb-driver/libusb-driver.gyp
@@ -11,6 +11,10 @@
'<(AOS)/common/util/util.gyp:thread',
'libusb_wrap',
'<(AOS)/build/aos.gyp:logging',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ 'export_dependent_settings': [
+ '<(AOS)/common/common.gyp:time',
],
},
{
diff --git a/gyro_board/src/libusb-driver/libusb_wrap.cc b/gyro_board/src/libusb-driver/libusb_wrap.cc
index 83cf245..0026ef3 100644
--- a/gyro_board/src/libusb-driver/libusb_wrap.cc
+++ b/gyro_board/src/libusb-driver/libusb_wrap.cc
@@ -1,5 +1,7 @@
#include "libusb_wrap.h"
+#include <string.h>
+
#include <iostream>
#include "aos/common/logging/logging.h"
@@ -115,8 +117,9 @@
Transfer::Transfer(size_t data_length,
void (*callback)(Transfer *, void *),
- void *user_data)
- : transfer_(libusb_alloc_transfer(0)),
+ void *user_data,
+ int num_iso_packets)
+ : transfer_(libusb_alloc_transfer(num_iso_packets)),
data_(new uint8_t[data_length]),
data_length_(data_length),
callback_(callback),
@@ -146,16 +149,16 @@
if (ret == LIBUSB_ERROR_BUSY) {
LOG(FATAL, "transfer %p already submitted\n", this);
}
- LOG(FATAL, "libusb error %d submitting transfer %p\n",
- ret, this);
+ LOG(FATAL, "libusb error %d submitting transfer %p. errno %d: %s\n",
+ ret, this, errno, strerror(errno));
}
}
void Transfer::Cancel() {
int ret = libusb_cancel_transfer(transfer_);
if (ret != 0) {
- LOG(FATAL, "libusb error %d cancelling transfer %p\n",
- ret, this);
+ LOG(FATAL, "libusb error %d cancelling transfer %p. errno %d: %s\n",
+ ret, this, errno, strerror(errno));
}
}
@@ -167,17 +170,14 @@
int num_packets,
void (*callback)(Transfer *, void *),
void *user_data)
- : Transfer(packet_length * num_packets, callback, user_data),
+ : Transfer(packet_length * num_packets, callback, user_data, num_packets),
num_packets_(num_packets) {
}
void IsochronousTransfer::FillIsochronous(LibUSBDeviceHandle *device,
unsigned char endpoint,
- unsigned int timeout) {
- (void)device;
- (void)endpoint;
- (void)timeout;
- /*libusb_fill_iso_transfer(transfer_,
+ const ::aos::time::Time &timeout) {
+ libusb_fill_iso_transfer(transfer_,
device->dev_handle_,
endpoint,
data_,
@@ -185,7 +185,8 @@
num_packets_,
StaticTransferCallback,
this,
- timeout);*/
+ timeout.ToMSec());
+ transfer_->iso_packet_desc[0].length = data_length_;
}
} // namespace libusb
diff --git a/gyro_board/src/libusb-driver/libusb_wrap.h b/gyro_board/src/libusb-driver/libusb_wrap.h
index 261cded..6056c66 100644
--- a/gyro_board/src/libusb-driver/libusb_wrap.h
+++ b/gyro_board/src/libusb-driver/libusb_wrap.h
@@ -4,6 +4,7 @@
#include <libusb-1.0/libusb.h>
#include "aos/common/macros.h"
+#include "aos/common/time.h"
class LibUSBDeviceHandle;
namespace libusb {
@@ -61,7 +62,8 @@
public:
Transfer(size_t data_length,
void (*callback)(Transfer *, void *),
- void *user_data);
+ void *user_data,
+ int num_iso_packets = 0);
~Transfer();
void FillInterrupt(LibUSBDeviceHandle *device,
@@ -72,7 +74,7 @@
void Cancel();
libusb_transfer_status status() { return transfer_->status; }
- int read_bytes() { return transfer_->actual_length; }
+ virtual int read_bytes() { return transfer_->actual_length; }
const uint8_t *data() { return data_; }
@@ -81,20 +83,22 @@
static_cast<Transfer *>(self->user_data)->TransferCallback();
}
- private:
- void TransferCallback();
-
+ protected:
libusb_transfer *const transfer_;
uint8_t *const data_;
size_t data_length_;
+ private:
+ void TransferCallback();
+
void (*const callback_)(Transfer *, void *);
void *const user_data_;
DISALLOW_COPY_AND_ASSIGN(Transfer);
};
+// TODO(brians): Make this actually work for num_packets != 1.
class IsochronousTransfer : public Transfer {
public:
IsochronousTransfer(size_t packet_length,
@@ -104,7 +108,11 @@
void FillIsochronous(LibUSBDeviceHandle *device,
unsigned char endpoint,
- unsigned int timeout);
+ const ::aos::time::Time &timeout);
+
+ virtual int read_bytes() {
+ return transfer_->iso_packet_desc[0].actual_length;
+ }
private:
const int num_packets_;
diff --git a/gyro_board/src/usb/CAN.c b/gyro_board/src/usb/CAN.c
index 6770328..591c6fc 100644
--- a/gyro_board/src/usb/CAN.c
+++ b/gyro_board/src/usb/CAN.c
@@ -10,7 +10,6 @@
#include "flash.h"
#include "partest.h"
#include "analog.h"
-#include "spi.h"
#include "LPCUSB/usbapi.h"
#include "CAN.h"
diff --git a/gyro_board/src/usb/FreeRTOSConfig.h b/gyro_board/src/usb/FreeRTOSConfig.h
index 5a46dad..57f81ab 100644
--- a/gyro_board/src/usb/FreeRTOSConfig.h
+++ b/gyro_board/src/usb/FreeRTOSConfig.h
@@ -69,7 +69,7 @@
#define configUSE_PREEMPTION 1
#define configUSE_IDLE_HOOK 0
#define configMAX_PRIORITIES ((unsigned portBASE_TYPE) 5)
-#define configUSE_TICK_HOOK 1
+#define configUSE_TICK_HOOK 0
#define configCPU_CLOCK_HZ ((unsigned long) 100000000)
#define configTICK_RATE_HZ ((portTickType ) 1000)
#define configMINIMAL_STACK_SIZE ((unsigned short) 80)
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
index 4609c96..8d4bff3 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
@@ -38,19 +38,20 @@
#include "LPC17xx.h"
-#include "analog.h"
+#include "fill_packet.h"
#define usbMAX_SEND_BLOCK ( 20 / portTICK_RATE_MS )
#define usbRXBUFFER_LEN ( 80 )
#define usbTXBUFFER_LEN ( 600 )
-#define INT_IN_EP 0x81 //read manual for picking these...
-#define INT_OUT_EP 0x04
+// Read the processor manual for picking these.
#define BULK_IN_EP 0x82
#define BULK_OUT_EP 0x05
#define ISOC_IN_EP 0x83
+#define NUM_ENDPOINTS 3
#define MAX_PACKET_SIZE 64
+#define DATA_PACKET_SIZE DATA_STRUCT_SEND_SIZE
#define LE_WORD(x) ((x)&0xFF),((x)>>8)
@@ -58,8 +59,13 @@
static xQueueHandle xRxedChars = NULL, xCharsForTx = NULL;
-static const unsigned char abDescriptors[] = {
+// This gets cleared each time the ISR is entered and then checked as it's
+// returning so that we can still yield from the ISR to a woken task but not
+// from the middle of the ISR like it would be if this was checked in each
+// endpoint handler that needs it.
+static portBASE_TYPE higher_priority_task_woken;
+static const unsigned char abDescriptors[] = {
// Device descriptor
0x12,
DESC_DEVICE,
@@ -79,7 +85,7 @@
// Configuration descriptor
0x09,
DESC_CONFIGURATION,
- LE_WORD(46), // wTotalLength
+ LE_WORD(9 + 9 + 7 * NUM_ENDPOINTS), // wTotalLength
0x01, // bNumInterfaces
0x01, // bConfigurationValue
0x00, // iConfiguration
@@ -90,7 +96,7 @@
DESC_INTERFACE,
0x00, // bInterfaceNumber
0x00, // bAlternateSetting
- 0x05, // bNumEndPoints
+ NUM_ENDPOINTS, // bNumEndPoints
0x0A, // bInterfaceClass = data
0x00, // bInterfaceSubClass
0x00, // bInterfaceProtocol
@@ -98,7 +104,7 @@
// Debug EP OUT
0x07,
DESC_ENDPOINT,
- BULK_OUT_EP, // bEndpointAddress
+ BULK_OUT_EP, // bEndpointAddress
0x02, // bmAttributes = bulk
LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
0x00, // bInterval
@@ -109,28 +115,13 @@
0x02, // bmAttributes = bulk
LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
0x00, // bInterval
-// Data EP OUT
- 0x07,
- DESC_ENDPOINT,
- INT_OUT_EP, // bEndpointAddress
- 0x03, // bmAttributes = intr
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x01, // bInterval
-// Data EP in
- 0x07,
- DESC_ENDPOINT,
- INT_IN_EP, // bEndpointAddress
- 0x03, // bmAttributes = intr
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x01, // bInterval
-
// isoc data EP IN
0x07,
DESC_ENDPOINT,
ISOC_IN_EP, // bEndpointAddress
0x0D, // bmAttributes = isoc, synchronous, data endpoint
- LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize
- 0x01, // bInterval
+ LE_WORD(DATA_PACKET_SIZE), // wMaxPacketSize
+ 0x01, // bInterval
// string descriptors
0x04,
@@ -162,7 +153,6 @@
*/
static void DebugOut(unsigned char bEP, unsigned char bEPStatus) {
int i, iLen;
- long lHigherPriorityTaskWoken = pdFALSE;
unsigned char abBulkBuf[64];
(void) bEPStatus;
@@ -171,10 +161,8 @@
iLen = USBHwEPRead(bEP, abBulkBuf, sizeof(abBulkBuf));
for (i = 0; i < iLen; i++) {
// put into queue
- xQueueSendFromISR(xRxedChars, &(abBulkBuf[ i ]), &lHigherPriorityTaskWoken);
+ xQueueSendFromISR(xRxedChars, &abBulkBuf[i], &higher_priority_task_woken);
}
-
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
}
@@ -186,7 +174,6 @@
*/
static void DebugIn(unsigned char bEP, unsigned char bEPStatus) {
int i, iLen;
- long lHigherPriorityTaskWoken = pdFALSE;
unsigned char abBulkBuf[64];
(void) bEPStatus;
@@ -199,8 +186,8 @@
// get bytes from transmit FIFO into intermediate buffer
for (i = 0; i < MAX_PACKET_SIZE; i++) {
- if (xQueueReceiveFromISR(xCharsForTx, (&abBulkBuf[i]),
- &lHigherPriorityTaskWoken) != pdPASS) {
+ if (xQueueReceiveFromISR(xCharsForTx, &abBulkBuf[i],
+ &higher_priority_task_woken) != pdPASS) {
break;
}
}
@@ -210,34 +197,9 @@
if (iLen > 0) {
USBHwEPWrite(bEP, abBulkBuf, iLen);
}
-
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
}
-static unsigned char abDataBuf[64];
-int VCOM_putcharFromISR(int c, long *woken);
-static void DataOut(unsigned char bEP, unsigned char bEPStatus) {
- int iLen;
- long lHigherPriorityTaskWoken = pdFALSE;
- /*
- char *a = "hello\n";
- while(*a){
- VCOM_putcharFromISR(*a,&lHigherPriorityTaskWoken);
- a ++;
- }
- */
- iLen = USBHwEPRead(bEP, abDataBuf, sizeof(abDataBuf));
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
-}
-
-static void DataIn(unsigned char bEP, unsigned char bEPStatus) {
- long lHigherPriorityTaskWoken = pdFALSE;
- fillSensorPacket(&usbPacket);
- USBHwEPWrite(bEP, (unsigned char *)&usbPacket, sizeof(usbPacket));
- portEND_SWITCHING_ISR(lHigherPriorityTaskWoken);
-}
-
/**
* Writes one character to VCOM port
*
@@ -289,26 +251,28 @@
* Simply calls the USB ISR
*/
void USB_IRQHandler(void) {
+ higher_priority_task_woken = pdFALSE;
USBHwISR();
+ portEND_SWITCHING_ISR(higher_priority_task_woken);
}
static void USBFrameHandler(unsigned short wFrame) {
(void) wFrame;
if (uxQueueMessagesWaitingFromISR(xCharsForTx) > 0) {
- // data available, enable interrupt instead of NAK on bulk in too
+ // Data to send is available so enable interrupt instead of NAK on bulk in
+ // too.
USBHwNakIntEnable(INACK_BI | INACK_II);
} else {
USBHwNakIntEnable(INACK_II);
}
fillSensorPacket(&usbPacket);
- USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&usbPacket, sizeof(usbPacket));
+ static uint8_t sequence = 0;
+ usbPacket.sequence = sequence++;
+ USBHwEPWrite(ISOC_IN_EP, (unsigned char *)&usbPacket, DATA_PACKET_SIZE);
}
-void vUSBTask(void *pvParameters) {
- portTickType xLastFlashTime;
-
- (void) pvParameters;
+void usb_init(void) {
DBG("Initialising USB stack\n");
xRxedChars = xQueueCreate(usbRXBUFFER_LEN, sizeof(char));
@@ -320,7 +284,7 @@
vTaskDelete(NULL);
}
- // initialise stack
+ // Initialise the USB stack.
USBInit();
// register descriptors
@@ -331,17 +295,12 @@
// HandleClassRequest, abClassReqData);
// register endpoint handlers
- USBHwRegisterEPIntHandler(INT_IN_EP, DataIn);
- USBHwRegisterEPIntHandler(INT_OUT_EP, DataOut);
USBHwRegisterEPIntHandler(BULK_IN_EP, DebugIn);
USBHwRegisterEPIntHandler(BULK_OUT_EP, DebugOut);
// register frame handler
USBHwRegisterFrameHandler(USBFrameHandler);
- // enable bulk-in interrupts on NAKs
- USBHwNakIntEnable(INACK_BI);
-
DBG("Starting USB communication\n");
NVIC_SetPriority(USB_IRQn, configUSB_INTERRUPT_PRIORITY);
@@ -352,20 +311,6 @@
DBG("Connecting to USB bus\n");
USBHwConnect(TRUE);
- xLastFlashTime = xTaskGetTickCount();
-
- vTaskDelayUntil(&xLastFlashTime, 1000 / portTICK_RATE_MS * 100);
-
- //USBHwAllowConnect();
- // echo any character received (do USB stuff in interrupt)
- for (;;) {
- // c = VCOM_getchar();
- // if (c != EOF) {
- // // Echo character back with INCREMENT_ECHO_BY offset, so for example if
- // // INCREMENT_ECHO_BY is 1 and 'A' is received, 'B' will be echoed back.
- // VCOM_putchar(c + INCREMENT_ECHO_BY);
- // }
- vTaskDelayUntil(&xLastFlashTime, 1000 / portTICK_RATE_MS);
- }
+ // Enable USB. The PC has probably disconnected it now.
+ USBHwAllowConnect();
}
-
diff --git a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
index 8229371..2fa038e 100644
--- a/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
+++ b/gyro_board/src/usb/LPCUSB/usbhw_lpc.c
@@ -458,6 +458,8 @@
// endpoint interrupt
if (dwStatus & EP_SLOW) {
// clear EP_SLOW
+ // TODO(brians): The manual says that this should happen after clearing
+ // stuff using USBEpIntClr.
USB->USBDevIntClr = EP_SLOW;
// check all endpoints
for (i = 0; i < 32; i++) {
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index adec079..2dc2286 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -35,6 +35,8 @@
FreeRTOS/portable/MemMang/heap_2.c \
alloc.c \
analog.c \
+ digital.c \
+ encoder.c \
FreeRTOS/portable/GCC/ARM_CM3/port.c \
FreeRTOS/tasks.c \
FreeRTOS/list.c \
@@ -44,7 +46,7 @@
LPCUSB/usbcontrol.c \
LPCUSB/USB_SENSOR_STREAM.c \
LPCUSB/usbhw_lpc.c \
- spi.c \
+ gyro.c \
LPCUSB/usbstdreq.c
all: $(NAME).hex
@@ -65,7 +67,7 @@
reset: deploy
# Echo an ESC into it to immediately exit the terminal.
- echo -e '\x1B' | $(FLASHER) -termonly -control $(PORT) $(SPEED) $(OSC)
+ `which echo` -e '\e' | $(FLASHER) -termonly -control $(PORT) $(SPEED) $(OSC)
cat:
@cd ../../bin; python serial_looper.py
diff --git a/gyro_board/src/usb/ParTest.c b/gyro_board/src/usb/ParTest.c
index 67f3a45..fa979cd 100644
--- a/gyro_board/src/usb/ParTest.c
+++ b/gyro_board/src/usb/ParTest.c
@@ -74,12 +74,9 @@
{
/* LEDs on port 2. */
GPIO2->FIODIR |= partstFIO1_BITS;
- //GPIO0->FIODIR = LED_4;
/* Start will all LEDs off. */
- GPIO2->FIOCLR = partstFIO1_BITS;
- //GPIO0->FIOSET = LED_4;
- //PINCON->PINMODE0 = (PINCON->PINMODE0 & 0xfffffff0) | 0x00000001;
+ GPIO2->FIOSET = partstFIO1_BITS;
}
/*-----------------------------------------------------------*/
@@ -116,4 +113,3 @@
}
}
/*-----------------------------------------------------------*/
-
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index 2b553c0..87d67ad 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -2,20 +2,12 @@
// CopyLeft qwerk Robotics unINC. 2010 All Rights Reserved.
// ****************************************************************************
-// ****************************************************************************
-// **************** IO Pin Setup
-// ****************************************************************************
+#include "analog.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
-#include "analog.h"
-
-// How long (in ms) to wait after a falling edge on the bottom indexer sensor
-// before reading the indexer encoder.
-static const int kBottomFallDelayTime = 32;
-
void analog_init(void) {
// b[1:0] CAN RD1 p0.0
// b[3:2] CAN TD1 p0.1
@@ -47,15 +39,7 @@
ADC->ADCR = 0x00200500;
}
-// ****************************************************************************
-// **************** ADC Functions
-// ****************************************************************************
-
-// **************** macros
-// starts conversion [26:24] = 001
-
-// **************** functions
int analog(int channel) {
ADC->ADCR = ((ADC->ADCR & 0xF8FFFF00) | (0x01000000 | (1 << channel)));
@@ -64,550 +48,3 @@
return ((ADC->ADGDR & 0x0000FFF0) >> 4);
}
-// GPIO1 P0.4
-// GPIO2 P0.5
-// GPIO3 P0.6
-// GPIO4 P0.7
-// GPIO5 P0.8
-// GPIO6 P0.9
-// GPIO7 P2.0
-// GPIO8 P2.1
-// GPIO9 P2.2
-// GPIO10 P2.3
-// GPIO11 P2.4
-// GPIO12 P2.5
-
-// DIP0 P1.29
-// DIP1 P2.13
-// DIP2 P0.11
-// DIP3 P0.10
-#define readGPIO(gpio, chan) ((((gpio)->FIOPIN) >> (chan)) & 1)
-inline int readGPIO_inline(int major, int minor) {
- switch (major) {
- case 0:
- return readGPIO(GPIO0, minor);
- case 1:
- return readGPIO(GPIO1, minor);
- case 2:
- return readGPIO(GPIO2, minor);
- default:
- return -1;
- }
-}
-int digital(int channel) {
- if (channel < 1) {
- return -1;
- } else if (channel < 7) {
- int chan = channel + 3;
- return readGPIO(GPIO0, chan);
- } else if (channel < 13) {
- int chan = channel - 7;
- return readGPIO(GPIO2, chan);
- }
- return -1;
-}
-int dip(int channel) {
- switch (channel) {
- case 0:
- return readGPIO(GPIO1, 29);
- case 1:
- return readGPIO(GPIO2, 13);
- case 2:
- return readGPIO(GPIO0, 11);
- case 3:
- return readGPIO(GPIO0, 10);
- default:
- return -1;
- }
-}
-// ENC0A 1.20
-// ENC0B 1.23
-// ENC1A 2.11
-// ENC1B 2.12
-// ENC2A 0.21
-// ENC2B 0.22
-// ENC3A 0.19
-// ENC3B 0.20
-
-#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
-int encoder_bits(int channel) {
- switch (channel) {
- case 0:
- return ENC(GPIO1, 20, 23);
- case 1:
- return ENC(GPIO2, 11, 12);
- case 2:
- return ENC(GPIO0, 21, 22);
- case 3:
- return ENC(GPIO0, 19, 20);
- default:
- return -1;
- }
- return -1;
-}
-#undef ENC
-
-// Uses EINT1 and EINT2 on 2.11 and 2.12.
-volatile int32_t encoder1_val;
-// On GPIO pins 0.22 and 0.21.
-volatile int32_t encoder2_val;
-// On GPIO pins 0.20 and 0.19.
-volatile int32_t encoder3_val;
-// On GPIO pins 2.0 and 2.1.
-volatile int32_t encoder4_val;
-// On GPIO pins 2.2 and 2.3.
-volatile int32_t encoder5_val;
-
-// ENC1A 2.11
-void EINT1_IRQHandler(void) {
- SC->EXTINT = 0x2;
- int fiopin = GPIO2->FIOPIN;
- if (((fiopin >> 1) ^ fiopin) & 0x800) {
- ++encoder1_val;
- } else {
- --encoder1_val;
- }
- SC->EXTPOLAR ^= 0x2;
-}
-// ENC1B 2.12
-void EINT2_IRQHandler(void) {
- SC->EXTINT = 0x4;
- int fiopin = GPIO2->FIOPIN;
- if (((fiopin >> 1) ^ fiopin) & 0x800) {
- --encoder1_val;
- } else {
- ++encoder1_val;
- }
- SC->EXTPOLAR ^= 0x4;
-}
-
-// GPIO Interrupt handlers
-static void NoGPIO() {}
-static void Encoder2ARise() {
- GPIOINT->IO0IntClr |= (1 << 22);
- if (GPIO0->FIOPIN & (1 << 21)) {
- ++encoder2_val;
- } else {
- --encoder2_val;
- }
-}
-static void Encoder2AFall() {
- GPIOINT->IO0IntClr |= (1 << 22);
- if (GPIO0->FIOPIN & (1 << 21)) {
- --encoder2_val;
- } else {
- ++encoder2_val;
- }
-}
-static void Encoder2BRise() {
- GPIOINT->IO0IntClr |= (1 << 21);
- if (GPIO0->FIOPIN & (1 << 22)) {
- --encoder2_val;
- } else {
- ++encoder2_val;
- }
-}
-static void Encoder2BFall() {
- GPIOINT->IO0IntClr |= (1 << 21);
- if (GPIO0->FIOPIN & (1 << 22)) {
- ++encoder2_val;
- } else {
- --encoder2_val;
- }
-}
-
-static void Encoder3ARise() {
- GPIOINT->IO0IntClr |= (1 << 20);
- if (GPIO0->FIOPIN & (1 << 19)) {
- ++encoder3_val;
- } else {
- --encoder3_val;
- }
-}
-static void Encoder3AFall() {
- GPIOINT->IO0IntClr |= (1 << 20);
- if (GPIO0->FIOPIN & (1 << 19)) {
- --encoder3_val;
- } else {
- ++encoder3_val;
- }
-}
-static void Encoder3BRise() {
- GPIOINT->IO0IntClr |= (1 << 19);
- if (GPIO0->FIOPIN & (1 << 20)) {
- --encoder3_val;
- } else {
- ++encoder3_val;
- }
-}
-static void Encoder3BFall() {
- GPIOINT->IO0IntClr |= (1 << 19);
- if (GPIO0->FIOPIN & (1 << 20)) {
- ++encoder3_val;
- } else {
- --encoder3_val;
- }
-}
-
-static void Encoder4ARise() {
- GPIOINT->IO2IntClr |= (1 << 0);
- if (GPIO2->FIOPIN & (1 << 1)) {
- ++encoder4_val;
- } else {
- --encoder4_val;
- }
-}
-static void Encoder4AFall() {
- GPIOINT->IO2IntClr |= (1 << 0);
- if (GPIO2->FIOPIN & (1 << 1)) {
- --encoder4_val;
- } else {
- ++encoder4_val;
- }
-}
-static void Encoder4BRise() {
- GPIOINT->IO2IntClr |= (1 << 1);
- if (GPIO2->FIOPIN & (1 << 0)) {
- --encoder4_val;
- } else {
- ++encoder4_val;
- }
-}
-static void Encoder4BFall() {
- GPIOINT->IO2IntClr |= (1 << 1);
- if (GPIO2->FIOPIN & (1 << 0)) {
- ++encoder4_val;
- } else {
- --encoder4_val;
- }
-}
-
-static void Encoder5ARise() {
- GPIOINT->IO2IntClr |= (1 << 2);
- if (GPIO2->FIOPIN & (1 << 3)) {
- ++encoder5_val;
- } else {
- --encoder5_val;
- }
-}
-static void Encoder5AFall() {
- GPIOINT->IO2IntClr |= (1 << 2);
- if (GPIO2->FIOPIN & (1 << 3)) {
- --encoder5_val;
- } else {
- ++encoder5_val;
- }
-}
-static void Encoder5BRise() {
- GPIOINT->IO2IntClr |= (1 << 3);
- if (GPIO2->FIOPIN & (1 << 2)) {
- --encoder5_val;
- } else {
- ++encoder5_val;
- }
-}
-static void Encoder5BFall() {
- GPIOINT->IO2IntClr |= (1 << 3);
- if (GPIO2->FIOPIN & (1 << 2)) {
- ++encoder5_val;
- } else {
- --encoder5_val;
- }
-}
-
-volatile int32_t capture_top_rise;
-volatile int8_t top_rise_count;
-static void IndexerTopRise() {
- GPIOINT->IO0IntClr |= (1 << 5);
- // edge counting encoder capture
- ++top_rise_count;
- capture_top_rise = encoder3_val;
-}
-volatile int32_t capture_top_fall;
-volatile int8_t top_fall_count;
-static void IndexerTopFall() {
- GPIOINT->IO0IntClr |= (1 << 5);
- // edge counting encoder capture
- ++top_fall_count;
- capture_top_fall = encoder3_val;
-}
-volatile int8_t bottom_rise_count;
-static void IndexerBottomRise() {
- GPIOINT->IO0IntClr |= (1 << 4);
- // edge counting
- ++bottom_rise_count;
-}
-volatile int32_t capture_bottom_fall_delay;
-volatile int8_t bottom_fall_delay_count;
-volatile int32_t dirty_delay;
-portTickType xDelayTimeFrom;
-static portTASK_FUNCTION(vDelayCapture, pvParameters)
-{
- portTickType xSleepFrom = xTaskGetTickCount();
-
- for (;;) {
- NVIC_DisableIRQ(EINT3_IRQn);
- if (dirty_delay != 0) {
- xSleepFrom = xDelayTimeFrom;
- dirty_delay = 0;
- NVIC_EnableIRQ(EINT3_IRQn);
-
- vTaskDelayUntil(&xSleepFrom, kBottomFallDelayTime / portTICK_RATE_MS);
-
- NVIC_DisableIRQ(EINT3_IRQn);
- ++bottom_fall_delay_count;
- capture_bottom_fall_delay = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- } else {
- NVIC_EnableIRQ(EINT3_IRQn);
- vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
- }
- }
-}
-
-volatile int8_t bottom_fall_count;
-static void IndexerBottomFall() {
- GPIOINT->IO0IntClr |= (1 << 4);
- ++bottom_fall_count;
- // edge counting start delayed capture
- xDelayTimeFrom = xTaskGetTickCount();
- dirty_delay = 1;
-}
-volatile int32_t capture_wrist_rise;
-volatile int8_t wrist_rise_count;
-static void WristHallRise() {
- GPIOINT->IO0IntClr |= (1 << 6);
- // edge counting encoder capture
- ++wrist_rise_count;
- capture_wrist_rise = (int32_t)QEI->QEIPOS;
-}
-volatile int32_t capture_shooter_angle_rise;
-volatile int8_t shooter_angle_rise_count;
-static void ShooterHallRise() {
- GPIOINT->IO0IntClr |= (1 << 7);
- // edge counting encoder capture
- ++shooter_angle_rise_count;
- capture_shooter_angle_rise = encoder2_val;
-}
-
-// Count leading zeros.
-// Returns 0 if bit 31 is set etc.
-__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
- uint32_t result;
- __asm__("clz %0, %1" : "=r" (result) : "r" (value));
- return result;
-}
-inline static void IRQ_Dispatch(void) {
- // TODO(brians): think about adding a loop here so that we can handle multiple
- // interrupts right on top of each other faster
- uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
- (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
-
- typedef void (*Handler)(void);
- const static Handler table[] = {
- Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
- Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
- Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
- Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
- NoGPIO, // index 4: NO GPIO #bit 27
- Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
- Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
- Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
- Encoder3BFall, // index 8: P0.19 Fall #bit 23 //Encoder 3 B
- Encoder2ARise, // index 9: P0.22 Rise #bit 22 //Encoder 2 A
- Encoder2BRise, // index 10: P0.21 Rise #bit 21 //Encoder 2 B
- Encoder3ARise, // index 11: P0.20 Rise #bit 20 //Encoder 3 A
- Encoder3BRise, // index 12: P0.19 Rise #bit 19 //Encoder 3 B
- NoGPIO, // index 13: NO GPIO #bit 18
- NoGPIO, // index 14: NO GPIO #bit 17
- NoGPIO, // index 15: NO GPIO #bit 16
- NoGPIO, // index 16: NO GPIO #bit 15
- NoGPIO, // index 17: NO GPIO #bit 14
- NoGPIO, // index 18: NO GPIO #bit 13
- NoGPIO, // index 19: NO GPIO #bit 12
- ShooterHallRise, // index 20: P0.7 Fall #bit 11 //Shooter Hall //Dio 4
- WristHallRise, // index 21: P0.6 Fall #bit 10 //Wrist Hall //Dio 3
- IndexerTopRise, // index 22: P0.5 Fall #bit 9 //Indexer Top //Dio 2
- IndexerBottomRise, // index 23: P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
- NoGPIO, // index 24: NO GPIO #bit 7
- NoGPIO, // index 25: NO GPIO #bit 6
- IndexerTopFall, // index 26: P0.5 Rise #bit 5 //Indexer Top //Dio 2
- IndexerBottomFall, // index 27: P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
- Encoder5BRise, // index 28: P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
- Encoder5ARise, // index 29: P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
- Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
- Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
- NoGPIO // index 32: NO BITS SET #False Alarm
- };
- table[index]();
-}
-void EINT3_IRQHandler(void) {
- IRQ_Dispatch();
-}
-int32_t encoder_val(int chan) {
- int32_t val;
- switch (chan) {
- case 0: // Wrist
- return (int32_t)QEI->QEIPOS;
- case 1: // Shooter Wheel
- NVIC_DisableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT2_IRQn);
- val = encoder1_val;
- NVIC_EnableIRQ(EINT2_IRQn);
- NVIC_EnableIRQ(EINT1_IRQn);
- return val;
- case 2: // Shooter Angle
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder2_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 3: // Indexer
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 4: // Drive R
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder4_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 5: // Drive L
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder5_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- default:
- return -1;
- }
-}
-void fillSensorPacket(struct DataStruct *packet) {
- packet->gyro_angle = gyro_angle;
-
- packet->shooter = encoder1_val;
- packet->left_drive = encoder5_val;
- packet->right_drive = encoder4_val;
- packet->shooter_angle = encoder2_val;
- packet->indexer = encoder3_val;
-
- NVIC_DisableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT2_IRQn);
-
- packet->wrist = (int32_t)QEI->QEIPOS;
- packet->wrist_hall_effect = !digital(3);
- packet->capture_wrist_rise = capture_wrist_rise;
- packet->wrist_rise_count = wrist_rise_count;
-
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
-
- NVIC_DisableIRQ(EINT3_IRQn);
-
- packet->capture_top_rise = capture_top_rise;
- packet->top_rise_count = top_rise_count;
- packet->capture_top_fall = capture_top_fall;
- packet->top_fall_count = top_fall_count;
- packet->top_disc = !digital(2);
-
- packet->capture_bottom_fall_delay = capture_bottom_fall_delay;
- packet->bottom_fall_delay_count = bottom_fall_delay_count;
- packet->bottom_fall_count = bottom_fall_count;
- packet->bottom_disc = !digital(1);
-
- packet->loader_top = !digital(5);
-
- packet->capture_shooter_angle_rise = capture_shooter_angle_rise;
- packet->shooter_angle_rise_count = shooter_angle_rise_count;
- packet->angle_adjust_bottom_hall_effect = !digital(4);
-
- NVIC_EnableIRQ(EINT3_IRQn);
-
- packet->bottom_rise_count = bottom_rise_count;
-}
-
-void encoder_init(void) {
- // Setup the encoder interface.
- SC->PCONP |= PCONP_PCQEI;
- PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
- // Reset the count and velocity.
- QEI->QEICON = 0x00000005;
- QEI->QEICONF = 0x00000004;
- // Wrap back to 0 when we wrap the int and vice versa.
- QEI->QEIMAXPOS = 0xFFFFFFFF;
-
- // Set up encoder 1.
- // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
- // PINSEL4[23:22] = {0 1}
- // PINSEL4[25:24] = {0 1}
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
- // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
- SC->EXTMODE = 0x6;
- SC->EXTINT = 0x6;
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
- encoder1_val = 0;
-
- // Set up encoder 2.
- GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
- GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 12);
- PINCON->PINSEL1 &= ~(0x3 << 10);
- encoder2_val = 0;
-
- // Set up encoder 3.
- GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
- GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 8);
- PINCON->PINSEL1 &= ~(0x3 << 6);
- encoder3_val = 0;
-
- // Set up encoder 4.
- GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 0);
- PINCON->PINSEL4 &= ~(0x3 << 2);
- encoder4_val = 0;
-
- // Set up encoder 5.
- GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 4);
- PINCON->PINSEL4 &= ~(0x3 << 6);
- encoder5_val = 0;
-
- // Enable interrupts from the GPIO pins.
- NVIC_EnableIRQ(EINT3_IRQn);
-
- xTaskCreate(vDelayCapture,
- (signed char *) "SENSORs",
- configMINIMAL_STACK_SIZE + 100,
- NULL /*parameters*/,
- tskIDLE_PRIORITY + 5,
- NULL /*return task handle*/);
-
- GPIOINT->IO0IntEnF |= (1 << 4); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 4); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 8);
-
- GPIOINT->IO0IntEnF |= (1 << 5); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 5); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 10);
-
- GPIOINT->IO0IntEnF |= (1 << 6);
- PINCON->PINSEL0 &= ~(0x3 << 12);
-
- GPIOINT->IO0IntEnF |= (1 << 7);
- PINCON->PINSEL0 &= ~(0x3 << 14);
-}
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index 8dc0963..1de38e7 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -1,64 +1,9 @@
#ifndef __ANALOG_H__
#define __ANALOG_H__
-extern int64_t gyro_angle;
-
-struct DataStruct {
- int64_t gyro_angle;
-
- int32_t left_drive;
- int32_t right_drive;
- int32_t shooter_angle;
- int32_t shooter;
- int32_t indexer;
- int32_t wrist;
-
- int32_t capture_top_rise;
- int32_t capture_top_fall;
- int32_t capture_bottom_fall_delay;
- int32_t capture_wrist_rise;
- int32_t capture_shooter_angle_rise;
-
- int8_t top_rise_count;
-
- int8_t top_fall_count;
-
- int8_t bottom_rise_count;
-
- int8_t bottom_fall_delay_count;
- int8_t bottom_fall_count;
-
- int8_t wrist_rise_count;
-
- int8_t shooter_angle_rise_count;
-
- union {
- struct {
- uint8_t wrist_hall_effect : 1;
- uint8_t angle_adjust_bottom_hall_effect : 1;
- uint8_t top_disc : 1;
- uint8_t bottom_disc : 1;
- uint8_t loader_top : 1;
- };
- uint32_t digitals;
- };
-} __attribute__((__packed__));
-// Gets called in the USB data output ISR. Assumes that it will not be preempted
-// except by very high priority things.
-void fillSensorPacket(struct DataStruct *packet);
+#include <stdint.h>
void analog_init(void);
int analog(int channel);
-int digital(int channel);
-
-void encoder_init(void);
-// For debugging only.
-// Returns the current values of the inputs for the given encoder (as the low 2
-// bits).
-int encoder_bits(int channel);
-// Returns the current position of the given encoder.
-int32_t encoder_val(int channel);
-
-int dip(int channel);
#endif // __ANALOG_H__
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
new file mode 100644
index 0000000..e5b8e95
--- /dev/null
+++ b/gyro_board/src/usb/data_struct.h
@@ -0,0 +1,112 @@
+// This isn't really a header file. It's designed to be #included directly into
+// other code (possibly in a namespace or whatever), so it doesn't have include
+// guards.
+// This means that it can not #include anything else because it (sometimes) gets
+// #included inside a namespace.
+// In the gyro board code, fill_packet.h #includes this file.
+// In the fitpc code, frc971/input/gyro_board_data.h #includes this file.
+
+#pragma pack(push, 1)
+// Be careful with declaration order in here. ARM doesn't like unaligned
+// accesses!
+struct DATA_STRUCT_NAME {
+ int64_t gyro_angle;
+
+ union {
+ struct {
+ // Which robot (+version) the gyro board is sending out data for.
+ // We should keep this in the same place for all gyro board software
+ // versions so that the fitpc can detect when it's reading from a gyro
+ // board set up for a different robot than it is.
+ // 0 = 2013 competition/practice robot
+ // 1 = 2013 3rd robot
+ uint8_t robot_id;
+ // This information should also be kept in the same place from year to
+ // year so that the fitpc code can record the dip switch values when it
+ // detects the wrong robot id to make debugging easier.
+ union {
+ struct {
+ uint8_t dip_switch0 : 1;
+ uint8_t dip_switch1 : 1;
+ uint8_t dip_switch2 : 1;
+ uint8_t dip_switch3 : 1;
+ // If the current gyro_angle has been not updated because of a bad
+ // reading from the sensor.
+ uint8_t old_gyro_reading : 1;
+ // If we're not going to get any more good gyro_angles.
+ uint8_t bad_gyro : 1;
+ };
+ uint8_t base_status;
+ };
+ };
+ uint16_t header;
+ };
+
+ // This is a counter that gets incremented with each packet sent (and wraps
+ // around when it reaches 255).
+ uint8_t sequence;
+
+ union {
+ struct {
+ union {
+ struct {
+ uint8_t wrist_hall_effect : 1;
+ uint8_t angle_adjust_bottom_hall_effect : 1;
+ uint8_t top_disc : 1;
+ uint8_t bottom_disc : 1;
+ uint8_t loader_top : 1;
+ uint8_t loader_bottom : 1;
+ };
+ uint16_t booleans;
+ };
+ int32_t left_drive;
+ int32_t right_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+ int32_t indexer;
+ int32_t wrist;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+ int32_t capture_wrist_rise;
+ int32_t capture_shooter_angle_rise;
+
+ int8_t top_rise_count;
+
+ int8_t top_fall_count;
+
+ int8_t bottom_rise_count;
+
+ int8_t bottom_fall_delay_count;
+ int8_t bottom_fall_count;
+
+ int8_t wrist_rise_count;
+
+ int8_t shooter_angle_rise_count;
+ } main;
+
+ struct {
+ union {
+ struct {
+ };
+ uint16_t booleans;
+ };
+ uint32_t shooter_cycle_ticks;
+ } bot3;
+ };
+};
+#pragma pack(pop)
+
+// This is how big the isochronous packets that we're going to send are.
+// This number is more painful to change than the actual size of the struct
+// because the code on both ends has to agree on this (or at least that's what
+// Brian found empirically 2013-10-24).
+#define DATA_STRUCT_SEND_SIZE 128
+
+#ifdef __cplusplus
+// TODO(brians): Consider using C1X's _Static_assert once we have a compiler
+// (GCC 4.6) + flags that support it.
+static_assert(sizeof(DATA_STRUCT_NAME) <= DATA_STRUCT_SEND_SIZE,
+ "The sensor data structure is too big.");
+#endif // defined(__cplusplus)
diff --git a/gyro_board/src/usb/digital.c b/gyro_board/src/usb/digital.c
new file mode 100644
index 0000000..c468173
--- /dev/null
+++ b/gyro_board/src/usb/digital.c
@@ -0,0 +1,34 @@
+#include "digital.h"
+
+inline int readGPIO_inline(int major, int minor) {
+ switch (major) {
+ case 0:
+ return readGPIO(GPIO0, minor);
+ case 1:
+ return readGPIO(GPIO1, minor);
+ case 2:
+ return readGPIO(GPIO2, minor);
+ default:
+ return -1;
+ }
+}
+
+int dip_switch(int channel) {
+ switch (channel) {
+ case 0:
+ return readGPIO(GPIO1, 29);
+ case 1:
+ return readGPIO(GPIO2, 13);
+ case 2:
+ return readGPIO(GPIO0, 11);
+ case 3:
+ return readGPIO(GPIO0, 10);
+ default:
+ return -1;
+ }
+}
+
+int is_bot3;
+void digital_init(void) {
+ is_bot3 = 0;
+}
diff --git a/gyro_board/src/usb/digital.h b/gyro_board/src/usb/digital.h
new file mode 100644
index 0000000..3792038
--- /dev/null
+++ b/gyro_board/src/usb/digital.h
@@ -0,0 +1,59 @@
+#ifndef GYRO_BOARD_USB_DIGITAL_H_
+#define GYRO_BOARD_USB_DIGITAL_H_
+
+#include "FreeRTOS.h"
+
+#define readGPIO(gpio, chan) ((((gpio)->FIOPIN) >> (chan)) & 1)
+
+// These are the actual pin numbers for all of the digital I(/0) pins on the
+// board.
+//
+// GPIO1 P0.4
+// GPIO2 P0.5
+// GPIO3 P0.6
+// GPIO4 P0.7
+// GPIO5 P0.8
+// GPIO6 P0.9
+// GPIO7 P2.0
+// GPIO8 P2.1
+// GPIO9 P2.2
+// GPIO10 P2.3
+// GPIO11 P2.4
+// GPIO12 P2.5
+//
+// DIP0 P1.29
+// DIP1 P2.13
+// DIP2 P0.11
+// DIP3 P0.10
+//
+// ENC0A 1.20
+// ENC0B 1.23
+// ENC1A 2.11
+// ENC1B 2.12
+// ENC2A 0.21
+// ENC2B 0.22
+// ENC3A 0.19
+// ENC3B 0.20
+
+void digital_init(void);
+
+inline int digital(int channel) {
+ if (channel < 1) {
+ return -1;
+ } else if (channel < 7) {
+ int chan = channel + 3;
+ return readGPIO(GPIO0, chan);
+ } else if (channel < 13) {
+ int chan = channel - 7;
+ return readGPIO(GPIO2, chan);
+ }
+ return -1;
+}
+
+int dip_switch(int channel);
+
+// Boolean set by digital_init() which says whether or not this is the 3rd
+// robot.
+extern int is_bot3;
+
+#endif // GYRO_BOARD_USB_DIGITAL_H_
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
new file mode 100644
index 0000000..17903cf
--- /dev/null
+++ b/gyro_board/src/usb/encoder.c
@@ -0,0 +1,632 @@
+#include <string.h>
+
+#include "fill_packet.h"
+#include "encoder.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "digital.h"
+#include "analog.h"
+#include "gyro.h"
+
+// How long (in ms) to wait after a falling edge on the bottom indexer sensor
+// before reading the indexer encoder.
+static const int kBottomFallDelayTime = 32;
+// How long to wait for a revolution of the shooter wheel (on the third robot)
+// before said wheel is deemed "stopped". (In secs)
+static const uint8_t kWheelStopThreshold = 1;
+
+#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
+int encoder_bits(int channel) {
+ switch (channel) {
+ case 0:
+ return ENC(GPIO1, 20, 23);
+ case 1:
+ return ENC(GPIO2, 11, 12);
+ case 2:
+ return ENC(GPIO0, 21, 22);
+ case 3:
+ return ENC(GPIO0, 19, 20);
+ default:
+ return -1;
+ }
+ return -1;
+}
+#undef ENC
+
+// Uses EINT1 and EINT2 on 2.11 and 2.12.
+volatile int32_t encoder1_val;
+// On GPIO pins 0.22 and 0.21.
+volatile int32_t encoder2_val;
+// On GPIO pins 0.20 and 0.19.
+volatile int32_t encoder3_val;
+// On GPIO pins 2.0 and 2.1.
+volatile int32_t encoder4_val;
+// On GPIO pins 2.2 and 2.3.
+volatile int32_t encoder5_val;
+
+// It is important to clear the various interrupt flags first thing in the ISRs.
+// It doesn't seem to work otherwise, possibly because of the reason that Brian
+// found poking around online: caches on the bus make it so that the clearing of
+// the interrupt gets to the NVIC after the ISR returns, so it runs the ISR a
+// second time. Also, by clearing them early, if a second interrupt arrives from
+// the same source it will still get handled instead of getting lost.
+
+// ENC1A 2.11
+void EINT1_IRQHandler(void) {
+ // Make sure to change this BEFORE clearing the interrupt like the datasheet
+ // says you have to.
+ SC->EXTPOLAR ^= 0x2;
+ SC->EXTINT = 0x2;
+ int fiopin = GPIO2->FIOPIN;
+ // This looks like a weird way to XOR the 2 inputs, but it compiles down to
+ // just 2 instructions, which is hard to beat.
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ ++encoder1_val;
+ } else {
+ --encoder1_val;
+ }
+}
+// ENC1B 2.12
+void EINT2_IRQHandler(void) {
+ SC->EXTPOLAR ^= 0x4;
+ SC->EXTINT = 0x4;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ --encoder1_val;
+ } else {
+ ++encoder1_val;
+ }
+}
+
+static inline void reset_TC(void) {
+ TIM2->TCR |= (1 << 1); // Put it into reset.
+ while (TIM2->TC != 0) { // Wait for reset.
+ continue;
+ }
+ TIM2->TCR = 1; // Take it out of reset + make sure it's enabled.
+}
+
+// TIM2
+volatile uint32_t shooter_cycle_ticks;
+void TIMER2_IRQHandler(void) {
+ // Apparently, this handler runs regardless of a match or capture event.
+ if (TIM2->IR & (1 << 4)) {
+ // Capture
+ TIM2->IR = (1 << 3); // Clear the interrupt.
+
+ shooter_cycle_ticks = TIM2->CR0;
+
+ reset_TC();
+ } else if (TIM2->IR & 1) {
+ // Match
+ TIM2->IR = 1; // Clear the interrupt
+
+ // Assume shooter is stopped.
+ shooter_cycle_ticks = 0;
+
+ // Disable timer.
+ TIM2->TCR = 0;
+ }
+
+ // It will only handle one interrupt per run.
+ // If there is another interrupt pending, it won't be cleared, and the ISR
+ // will be run again to handle it.
+}
+
+// TODO(brians): Have this indicate some kind of error instead of just looping
+// infinitely in the ISR because it never clears it.
+static void NoGPIO(void) {}
+static void Encoder2ARise(void) {
+ GPIOINT->IO0IntClr = (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
+}
+static void Encoder2AFall(void) {
+ GPIOINT->IO0IntClr = (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
+}
+static void Encoder2BRise(void) {
+ GPIOINT->IO0IntClr = (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
+}
+static void Encoder2BFall(void) {
+ GPIOINT->IO0IntClr = (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
+}
+
+static void Encoder3ARise(void) {
+ GPIOINT->IO0IntClr = (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
+}
+static void Encoder3AFall(void) {
+ GPIOINT->IO0IntClr = (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
+}
+static void Encoder3BRise(void) {
+ GPIOINT->IO0IntClr = (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
+}
+static void Encoder3BFall(void) {
+ GPIOINT->IO0IntClr = (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
+}
+
+static void Encoder4ARise(void) {
+ GPIOINT->IO2IntClr = (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+static void Encoder4AFall(void) {
+ GPIOINT->IO2IntClr = (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+static void Encoder4BRise(void) {
+ GPIOINT->IO2IntClr = (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+static void Encoder4BFall(void) {
+ GPIOINT->IO2IntClr = (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+
+static void Encoder5ARise(void) {
+ GPIOINT->IO2IntClr = (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
+}
+static void Encoder5AFall(void) {
+ GPIOINT->IO2IntClr = (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
+}
+static void Encoder5BRise(void) {
+ GPIOINT->IO2IntClr = (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
+}
+static void Encoder5BFall(void) {
+ GPIOINT->IO2IntClr = (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
+}
+
+volatile int32_t capture_top_rise;
+volatile int8_t top_rise_count;
+static void IndexerTopRise(void) {
+ GPIOINT->IO0IntClr = (1 << 5);
+ // edge counting encoder capture
+ ++top_rise_count;
+ capture_top_rise = encoder3_val;
+}
+volatile int32_t capture_top_fall;
+volatile int8_t top_fall_count;
+static void IndexerTopFall(void) {
+ GPIOINT->IO0IntClr = (1 << 5);
+ // edge counting encoder capture
+ ++top_fall_count;
+ capture_top_fall = encoder3_val;
+}
+volatile int8_t bottom_rise_count;
+static void IndexerBottomRise(void) {
+ GPIOINT->IO0IntClr = (1 << 4);
+ // edge counting
+ ++bottom_rise_count;
+}
+volatile int32_t capture_bottom_fall_delay;
+volatile int8_t bottom_fall_delay_count;
+portTickType xDelayTimeFrom;
+static portTASK_FUNCTION(vDelayCapture, pvParameters)
+{
+ portTickType xSleepFrom = xTaskGetTickCount();
+
+ for (;;) {
+ // Atomically (wrt the ISR) switch xDelayTimeFrom to 0 and store its old
+ // value to use later.
+ NVIC_DisableIRQ(EINT3_IRQn);
+ portTickType new_time = xDelayTimeFrom;
+ xDelayTimeFrom = 0;
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ if (new_time != 0) {
+ xSleepFrom = new_time;
+
+ vTaskDelayUntil(&xSleepFrom, kBottomFallDelayTime / portTICK_RATE_MS);
+
+ // Make sure that the USB ISR doesn't look at inconsistent values.
+ NVIC_DisableIRQ(USB_IRQn);
+ capture_bottom_fall_delay = encoder3_val;
+ ++bottom_fall_delay_count;
+ NVIC_EnableIRQ(USB_IRQn);
+ } else {
+ // Wait 10ms and then check again.
+ vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
+ }
+ }
+}
+
+volatile int8_t bottom_fall_count;
+static void IndexerBottomFall(void) {
+ GPIOINT->IO0IntClr = (1 << 4);
+ ++bottom_fall_count;
+ // edge counting start delayed capture
+ xDelayTimeFrom = xTaskGetTickCount();
+}
+volatile int32_t capture_wrist_rise;
+volatile int8_t wrist_rise_count;
+static void WristHallRise(void) {
+ GPIOINT->IO0IntClr = (1 << 6);
+ // edge counting encoder capture
+ ++wrist_rise_count;
+ capture_wrist_rise = (int32_t)QEI->QEIPOS;
+}
+volatile int32_t capture_shooter_angle_rise;
+volatile int8_t shooter_angle_rise_count;
+static void ShooterHallRise(void) {
+ GPIOINT->IO0IntClr = (1 << 7);
+ // edge counting encoder capture
+ ++shooter_angle_rise_count;
+ capture_shooter_angle_rise = encoder2_val;
+}
+
+// Third robot shooter.
+static void ShooterPhotoFall(void) {
+ GPIOINT->IO0IntClr = (1 << 23);
+ // We reset TC to make sure we don't get a crap
+ // value from CR0 when the capture interrupt occurs
+ // if the shooter is just starting up again, and so
+ // that the match interrupt thing works right.
+ reset_TC();
+}
+
+typedef void (*Handler)(void);
+// Contains default pointers for ISR functions.
+// (These can be used without modifications on the comp/practice bots.)
+Handler ISRTable[] = {
+ Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
+ Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
+ Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
+ Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
+ NoGPIO, // index 4: NO GPIO #bit 27
+ Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
+ Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
+ Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
+ Encoder3BFall, // index 8: P0.19 Fall #bit 23 //Encoder 3 B
+ Encoder2ARise, // index 9: P0.22 Rise #bit 22 //Encoder 2 A
+ Encoder2BRise, // index 10: P0.21 Rise #bit 21 //Encoder 2 B
+ Encoder3ARise, // index 11: P0.20 Rise #bit 20 //Encoder 3 A
+ Encoder3BRise, // index 12: P0.19 Rise #bit 19 //Encoder 3 B
+ NoGPIO, // index 13: NO GPIO #bit 18
+ NoGPIO, // index 14: NO GPIO #bit 17
+ NoGPIO, // index 15: NO GPIO #bit 16
+ NoGPIO, // index 16: NO GPIO #bit 15
+ NoGPIO, // index 17: NO GPIO #bit 14
+ NoGPIO, // index 18: NO GPIO #bit 13
+ NoGPIO, // index 19: NO GPIO #bit 12
+ ShooterHallRise, // index 20: P0.7 Fall #bit 11 //Shooter Hall //Dio 4
+ WristHallRise, // index 21: P0.6 Fall #bit 10 //Wrist Hall //Dio 3
+ IndexerTopRise, // index 22: P0.5 Fall #bit 9 //Indexer Top //Dio 2
+ IndexerBottomRise, // index 23: P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
+ NoGPIO, // index 24: NO GPIO #bit 7
+ NoGPIO, // index 25: NO GPIO #bit 6
+ IndexerTopFall, // index 26: P0.5 Rise #bit 5 //Indexer Top //Dio 2
+ IndexerBottomFall, // index 27: P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
+ Encoder5BRise, // index 28: P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
+ Encoder5ARise, // index 29: P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
+ Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
+ Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
+ NoGPIO // index 32: NO BITS SET #False Alarm
+};
+
+// Count leading zeros.
+// Returns 0 if bit 31 is set etc.
+__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
+ uint32_t result;
+ __asm__("clz %0, %1" : "=r" (result) : "r" (value));
+ return result;
+}
+inline static void IRQ_Dispatch(void) {
+ // There is no need to add a loop here to handle multiple interrupts at the
+ // same time because the processor has tail chaining of interrupts which we
+ // can't really beat with our own loop.
+ // It would actually be bad because a loop here would block EINT1/2 for longer
+ // lengths of time.
+
+ uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
+ (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
+
+ ISRTable[index]();
+}
+void EINT3_IRQHandler(void) {
+ IRQ_Dispatch();
+}
+
+int32_t encoder_val(int chan) {
+ int32_t val;
+ switch (chan) {
+ case 0: // Wrist
+ return (int32_t)QEI->QEIPOS;
+ case 1: // Shooter Wheel
+ NVIC_DisableIRQ(EINT1_IRQn);
+ NVIC_DisableIRQ(EINT2_IRQn);
+ val = encoder1_val;
+ NVIC_EnableIRQ(EINT2_IRQn);
+ NVIC_EnableIRQ(EINT1_IRQn);
+ return val;
+ case 2: // Shooter Angle
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder2_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 3: // Indexer
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder3_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 4: // Drive R
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder4_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 5: // Drive L
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder5_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ default:
+ return -1;
+ }
+}
+
+void encoder_init(void) {
+ // Setup the encoder interface.
+ SC->PCONP |= PCONP_PCQEI;
+ PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
+ // Reset the count and velocity.
+ QEI->QEICON = 0x00000005;
+ QEI->QEICONF = 0x00000004;
+ // Wrap back to 0 when we wrap the int and vice versa.
+ QEI->QEIMAXPOS = 0xFFFFFFFF;
+
+ // Set up encoder 4.
+ GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 0);
+ PINCON->PINSEL4 &= ~(0x3 << 2);
+ encoder4_val = 0;
+
+ // Set up encoder 5.
+ GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 4);
+ PINCON->PINSEL4 &= ~(0x3 << 6);
+ encoder5_val = 0;
+
+ // Enable interrupts from the GPIO pins.
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ if (is_bot3) {
+ // Modify robot handler table for third robot.
+ ISRTable[23] = ShooterPhotoFall;
+
+ // Set up timer for bot3 photosensor.
+ // Make sure timer two is powered.
+ SC->PCONP |= (1 << 22);
+ // We don't need all the precision the CCLK can provide.
+ // We'll use CCLK/8. (12.5 mhz).
+ SC->PCLKSEL1 |= (0x3 << 12);
+ // Use timer prescale to get that freq down to 500 hz.
+ TIM2->PR = 25000;
+ // Select capture 2.0 function on pin 0.4.
+ PINCON->PINSEL0 |= (0x3 << 8);
+ // Set timer to capture and interrupt on rising edge.
+ TIM2->CCR = 0x5;
+ // Set up match interrupt.
+ TIM2->MR0 = kWheelStopThreshold * 500;
+ TIM2->MCR = 1;
+ // Enable timer IRQ, and make it lower priority than the encoders.
+ NVIC_SetPriority(TIMER3_IRQn, 1);
+ NVIC_EnableIRQ(TIMER3_IRQn);
+ // Set up GPIO interrupt on other edge.
+ GPIOINT->IO0IntEnF |= (1 << 23);
+
+ } else { // is main robot
+ // Set up encoder 1.
+ // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
+ // PINSEL4[23:22] = {0 1}
+ // PINSEL4[25:24] = {0 1}
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
+ // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
+ SC->EXTMODE = 0x6;
+ SC->EXTINT = 0x6;
+ NVIC_EnableIRQ(EINT1_IRQn);
+ NVIC_EnableIRQ(EINT2_IRQn);
+ encoder1_val = 0;
+
+ // Set up encoder 2.
+ GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL1 &= ~(0x3 << 12);
+ PINCON->PINSEL1 &= ~(0x3 << 10);
+ encoder2_val = 0;
+
+ // Set up encoder 3.
+ GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL1 &= ~(0x3 << 8);
+ PINCON->PINSEL1 &= ~(0x3 << 6);
+ encoder3_val = 0;
+
+ xTaskCreate(vDelayCapture,
+ (signed char *) "SENSORs",
+ configMINIMAL_STACK_SIZE + 100,
+ NULL /*parameters*/,
+ tskIDLE_PRIORITY + 5,
+ NULL /*return task handle*/);
+
+ GPIOINT->IO0IntEnF |= (1 << 4); // Set GPIO falling interrupt
+ GPIOINT->IO0IntEnR |= (1 << 4); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 8);
+
+ GPIOINT->IO0IntEnF |= (1 << 5); // Set GPIO falling interrupt
+ GPIOINT->IO0IntEnR |= (1 << 5); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 10);
+
+ GPIOINT->IO0IntEnF |= (1 << 6);
+ PINCON->PINSEL0 &= ~(0x3 << 12);
+
+ GPIOINT->IO0IntEnF |= (1 << 7);
+ PINCON->PINSEL0 &= ~(0x3 << 14);
+ }
+}
+
+void fillSensorPacket(struct DataStruct *packet) {
+ if (gyro_output.initialized) {
+ packet->gyro_angle = gyro_output.angle;
+ packet->old_gyro_reading = gyro_output.last_reading_bad;
+ packet->bad_gyro = gyro_output.gyro_bad;
+ } else {
+ packet->gyro_angle = 0;
+ packet->old_gyro_reading = 1;
+ packet->bad_gyro = 0;
+ }
+
+ packet->dip_switch0 = dip_switch(0);
+ packet->dip_switch1 = dip_switch(1);
+ packet->dip_switch2 = dip_switch(2);
+ packet->dip_switch3 = dip_switch(3);
+
+ // We disable EINT3 to avoid sending back inconsistent values. All of the
+ // aligned reads from the variables are atomic, so disabling it isn't
+ // necessary for just reading encoder values. We re-enable it periodically
+ // because disabling and enabling is cheap (2 instructions) and we really rely
+ // on low interrupt latencies.
+
+ packet->main.left_drive = encoder5_val;
+ packet->main.right_drive = encoder4_val;
+
+ if (is_bot3) {
+ packet->robot_id = 1;
+
+ packet->bot3.shooter_cycle_ticks = shooter_cycle_ticks;
+ } else { // is main robot
+ packet->robot_id = 0;
+
+ packet->main.shooter = encoder1_val;
+ packet->main.indexer = encoder3_val;
+
+ NVIC_DisableIRQ(EINT3_IRQn);
+
+ packet->main.wrist = (int32_t)QEI->QEIPOS;
+ packet->main.wrist_hall_effect = !digital(3);
+ packet->main.capture_wrist_rise = capture_wrist_rise;
+ packet->main.wrist_rise_count = wrist_rise_count;
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+ NVIC_DisableIRQ(EINT3_IRQn);
+
+ packet->main.capture_top_rise = capture_top_rise;
+ packet->main.top_rise_count = top_rise_count;
+ packet->main.capture_top_fall = capture_top_fall;
+ packet->main.top_fall_count = top_fall_count;
+ packet->main.top_disc = !digital(2);
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+ NVIC_DisableIRQ(EINT3_IRQn);
+
+ packet->main.capture_bottom_fall_delay = capture_bottom_fall_delay;
+ packet->main.bottom_fall_delay_count = bottom_fall_delay_count;
+ packet->main.bottom_fall_count = bottom_fall_count;
+ packet->main.bottom_disc = !digital(1);
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+ NVIC_DisableIRQ(EINT3_IRQn);
+
+ packet->main.loader_top = !digital(5);
+ packet->main.loader_bottom = !digital(6);
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+ NVIC_DisableIRQ(EINT3_IRQn);
+
+ packet->main.shooter_angle = encoder2_val;
+ packet->main.capture_shooter_angle_rise = capture_shooter_angle_rise;
+ packet->main.shooter_angle_rise_count = shooter_angle_rise_count;
+ packet->main.angle_adjust_bottom_hall_effect = !digital(4);
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ packet->main.bottom_rise_count = bottom_rise_count;
+ }
+}
diff --git a/gyro_board/src/usb/encoder.h b/gyro_board/src/usb/encoder.h
new file mode 100644
index 0000000..5b1f3ab
--- /dev/null
+++ b/gyro_board/src/usb/encoder.h
@@ -0,0 +1,16 @@
+#ifndef GYRO_BOARD_USB_ENCODER_H_
+#define GYRO_BOARD_USB_ENCODER_H_
+
+#include <stdint.h>
+
+void encoder_init(void);
+
+// For debugging only.
+// Returns the current values of the inputs for the given encoder (as the low 2
+// bits).
+int encoder_bits(int channel);
+
+// Returns the current position of the given encoder.
+int32_t encoder_val(int channel);
+
+#endif // GYRO_BOARD_USB_ENCODER_H_
diff --git a/gyro_board/src/usb/fill_packet.h b/gyro_board/src/usb/fill_packet.h
new file mode 100644
index 0000000..9acc094
--- /dev/null
+++ b/gyro_board/src/usb/fill_packet.h
@@ -0,0 +1,16 @@
+#ifndef GYRO_BOARD_FILL_PACKET_H_
+#define GYRO_BOARD_FILL_PACKET_H_
+
+#include <stdint.h>
+
+#define DATA_STRUCT_NAME DataStruct
+#include "data_struct.h"
+#undef DATA_STRUCT_NAME
+
+// Gets called in the USB data output ISR. Assumes that it will not be preempted
+// except by very high priority things.
+//
+// Implemented in encoder.c because it depends on so many things in there.
+void fillSensorPacket(struct DataStruct *packet);
+
+#endif // GYRO_BOARD_FILL_PACKET_H_
diff --git a/gyro_board/src/usb/gyro.c b/gyro_board/src/usb/gyro.c
new file mode 100644
index 0000000..866e890
--- /dev/null
+++ b/gyro_board/src/usb/gyro.c
@@ -0,0 +1,347 @@
+#include "gyro.h"
+
+#include <stdio.h>
+#include <inttypes.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "partest.h"
+
+struct GyroOutput gyro_output;
+
+static void gyro_disable_csel(void) {
+ // Set the CSEL pin high to deselect it.
+ GPIO0->FIOSET = 1 << 16;
+}
+
+static void gyro_enable_csel(void) {
+ // Clear the CSEL pin to select it.
+ GPIO0->FIOCLR = 1 << 16;
+}
+
+// Blocks until there is data available.
+static uint16_t spi_read(void) {
+ while (!(SSP0->SR & (1 << 2))) {}
+ return SSP0->DR;
+}
+
+// Blocks until there is space to enqueue data.
+static void spi_write(uint16_t data) {
+ while (!(SSP0->SR & (1 << 1))) {}
+ SSP0->DR = data;
+}
+
+static uint32_t do_gyro_read(uint32_t data, int *parity_error) {
+ *parity_error = 0;
+
+ gyro_enable_csel();
+ spi_write(data >> 16);
+ if (__builtin_parity(data & ~1) == 0) data |= 1;
+ spi_write(data);
+
+ uint16_t high_value = spi_read();
+ if (__builtin_parity(high_value) != 1) {
+ printf("high value 0x%"PRIx16" parity error\n", high_value);
+ *parity_error = 1;
+ }
+ uint16_t low_value = spi_read();
+ gyro_disable_csel();
+ uint32_t r = high_value << 16 | low_value;
+ if (__builtin_parity(r) != 1) {
+ printf("low value 0x%"PRIx16" parity error (r=%"PRIx32")\n", low_value, r);
+ *parity_error = 1;
+ }
+
+ return r;
+}
+
+// Returns all of the non-data bits in the "header" except the parity from
+// value.
+static uint8_t gyro_status(uint32_t value) {
+ return (value >> 26) & ~4;
+}
+
+// Returns all of the error bits in the "footer" from value.
+static uint8_t gyro_errors(uint32_t value) {
+ return (value >> 1) & 0x7F;
+}
+
+// Performs a read from the gyro.
+// Sets *bad_reading to 1 if the result is potentially bad and *bad_gyro to 1 if
+// the gyro is bad and we're not going to get any more readings.
+static int16_t gyro_read(int *bad_reading, int *bad_gyro) {
+ *bad_reading = *bad_gyro = 0;
+
+ int parity_error;
+ uint32_t value = do_gyro_read(0x20000000, &parity_error);
+
+ if (parity_error) {
+ *bad_reading = 1;
+ return 0;
+ }
+
+ // This check assumes that the sequence bits are all 0, but they should be
+ // because that's all we send.
+ if (gyro_status(value) != 1) {
+ uint8_t status = gyro_status(value);
+ if (status == 0) {
+ printf("gyro says sensor data is bad\n");
+ } else {
+ printf("gyro gave weird status 0x%"PRIx8"\n", status);
+ }
+ *bad_reading = 1;
+ }
+
+ if (gyro_errors(value) != 0) {
+ uint8_t errors = gyro_errors(value);
+ if (errors & ~(1 << 1)) {
+ *bad_reading = 1;
+ // Error 1 (continuous self-test error) will set status to 0 if it's bad
+ // enough by itself.
+ }
+ if (errors & (1 << 6)) {
+ printf("gyro PLL error\n");
+ }
+ if (errors & (1 << 5)) {
+ printf("gyro quadrature error\n");
+ }
+ if (errors & (1 << 4)) {
+ printf("gyro non-volatile memory error\n");
+ *bad_gyro = 1;
+ }
+ if (errors & (1 << 3)) {
+ printf("gyro volatile memory error\n");
+ *bad_gyro = 1;
+ }
+ if (errors & (1 << 2)) {
+ printf("gyro power error\n");
+ }
+ if (errors & (1 << 1)) {
+ printf("gyro continuous self-test error\n");
+ }
+ if (errors & 1) {
+ printf("gyro unexpected self check mode\n");
+ }
+ }
+ if (*bad_gyro) {
+ *bad_reading = 1;
+ return 0;
+ } else {
+ return -(int16_t)(value >> 10 & 0xFFFF);
+ }
+}
+
+// Returns 1 if the setup failed or 0 if it succeeded.
+static int gyro_setup(void) {
+ for (int i = 0; i < 100; ++i) {
+ portTickType wait_time = xTaskGetTickCount();
+ int parity_error;
+
+ // Wait for it to start up.
+ vTaskDelayUntil(&wait_time, 100 / portTICK_RATE_MS);
+ // Get it started doing a check.
+ uint32_t value = do_gyro_read(0x20000003, &parity_error);
+ if (parity_error) continue;
+ // Its initial response is hardcoded to 1.
+ if (value != 1) {
+ printf("gyro unexpected initial response 0x%"PRIx32"\n", value);
+ // There's a chance that we're retrying because of a parity error
+ // previously, so keep going.
+ }
+
+ // Wait for it to assert the fault conditions.
+ vTaskDelayUntil(&wait_time, 50 / portTICK_RATE_MS);
+ // Dummy read to clear the old latched state.
+ do_gyro_read(0x20000000, &parity_error);
+ if (parity_error) continue;
+
+ // Wait for it to clear the fault conditions.
+ vTaskDelayUntil(&wait_time, 50 / portTICK_RATE_MS);
+ value = do_gyro_read(0x20000000, &parity_error);
+ if (parity_error) continue;
+ // If it's not reporting self test data.
+ if (gyro_status(value) != 2) {
+ printf("gyro first value 0x%"PRIx32" not self test data\n", value);
+ continue;
+ }
+ // If we don't see all of the errors.
+ if (gyro_errors(value) != 0x7F) {
+ printf("gyro self test value 0x%"PRIx32" is bad\n", value);
+ return 1;
+ }
+
+ // Wait for the sequential transfer delay.
+ vTaskDelayUntil(&wait_time, 1 / portTICK_RATE_MS);
+ value = do_gyro_read(0x20000000, &parity_error);
+ if (parity_error) continue;
+ // It should still be reporting self test data.
+ if (gyro_status(value) != 2) {
+ printf("gyro second value 0x%"PRIx32" not self test data\n", value);
+ continue;
+ }
+ return 0;
+ }
+ return 1;
+}
+
+static portTASK_FUNCTION(gyro_read_task, pvParameters) {
+ // Connect power and clock.
+ SC->PCONP |= PCONP_PCSSP0;
+ SC->PCLKSEL1 &= ~(3 << 10);
+ SC->PCLKSEL1 |= 1 << 10;
+
+ // Set up SSEL.
+ // It's is just a GPIO pin because we're the master (it would be special if we
+ // were a slave).
+ gyro_disable_csel();
+ GPIO0->FIODIR |= 1 << 16;
+ PINCON->PINSEL1 &= ~(3 << 0);
+ PINCON->PINSEL1 |= 0 << 0;
+
+ // Set up MISO0 and MOSI0.
+ PINCON->PINSEL1 &= ~(3 << 2 | 3 << 4);
+ PINCON->PINSEL1 |= 2 << 2 | 2 << 4;
+
+ // Set up SCK0.
+ PINCON->PINSEL0 &= ~(3 << 30);
+ PINCON->PINSEL0 |= (2 << 30);
+
+ // Make sure it's disabled.
+ SSP0->CR1 = 0;
+ SSP0->CR0 =
+ 0xF /* 16 bit transfer */ |
+ 0 << 4 /* SPI mode */ |
+ 0 << 6 /* CPOL = 0 */ |
+ 0 << 7 /* CPHA = 0 */;
+ // 14 clocks per cycle. This works out to a ~7.2MHz bus.
+ // The gyro is rated for a maximum of 8.08MHz.
+ SSP0->CPSR = 14;
+ // Finally, enable it.
+ // This has to be done after we're done messing with everything else.
+ SSP0->CR1 |= 1 << 1;
+
+ if (gyro_setup()) {
+ printf("gyro setup failed. deleting task\n");
+ gyro_output.angle = 0;
+ gyro_output.last_reading_bad = gyro_output.gyro_bad = 1;
+ gyro_output.initialized = 1;
+ vTaskDelete(NULL);
+ return;
+ } else {
+ gyro_output.initialized = 1;
+ }
+
+ gyro_output.angle = 0;
+ gyro_output.last_reading_bad = 1; // until we're started up
+ gyro_output.gyro_bad = 0;
+
+ // How many times per second to read the gyro value.
+ static const int kGyroReadFrequency = 200;
+ // How many times per second to flash the LED.
+ // Must evenly divide kGyroReadFrequency.
+ static const int kFlashFrequency = 10;
+
+ static const int kStartupCycles = kGyroReadFrequency * 2;
+ static const int kZeroingCycles = kGyroReadFrequency * 6;
+
+ // An accumulator for all of the values read while zeroing.
+ int32_t zero_bias = 0;
+
+ int startup_cycles_left = kStartupCycles;
+ int zeroing_cycles_left = kZeroingCycles;
+
+ // These are a pair that hold the offset calculated while zeroing.
+ // full_units_ is the base (in ticks) and remainder_ ranges between 0 and
+ // kZeroingCycles (like struct timespec). remainder_ is used to calculate which
+ // cycles to add an additional unit to the result.
+ int32_t full_units_offset = 0;
+ int32_t remainder_offset = 0;
+ // This keeps track of when to add 1 to the read value (using _offset).
+ int32_t remainder_sum = 0;
+
+ int32_t led_flash = 0;
+ vParTestSetLED(0, 0);
+
+ portTickType xLastGyroReadTime = xTaskGetTickCount();
+
+ for (;;) {
+ ++led_flash;
+ if (led_flash < kGyroReadFrequency / kFlashFrequency / 2) {
+ vParTestSetLED(1, 0);
+ } else {
+ vParTestSetLED(1, 1);
+ }
+ if (led_flash >= kGyroReadFrequency / kFlashFrequency) {
+ led_flash = 0;
+ }
+
+ vTaskDelayUntil(&xLastGyroReadTime,
+ 1000 / kGyroReadFrequency / portTICK_RATE_MS);
+
+ int bad_reading, bad_gyro;
+ int16_t gyro_value = gyro_read(&bad_reading, &bad_gyro);
+ if (bad_gyro) {
+ // We're just going to give up if this happens (write out that we're
+ // giving up and then never run anything else in this task).
+ vParTestSetLED(0, 1);
+ printf("gyro read task giving up because of bad gyro\n");
+ portENTER_CRITICAL();
+ gyro_output.gyro_bad = 1;
+ gyro_output.last_reading_bad = 1;
+ gyro_output.angle = 0;
+ portEXIT_CRITICAL();
+ vTaskDelete(NULL);
+ while (1) {}
+ }
+
+ if (startup_cycles_left) {
+ vParTestSetLED(2, 0);
+ --startup_cycles_left;
+ if (bad_reading) {
+ printf("gyro retrying startup wait because of bad reading\n");
+ startup_cycles_left = kStartupCycles;
+ }
+ } else if (zeroing_cycles_left) {
+ vParTestSetLED(2, 1);
+ --zeroing_cycles_left;
+ if (bad_reading) {
+ printf("gyro restarting zeroing because of bad reading\n");
+ zeroing_cycles_left = kZeroingCycles;
+ zero_bias = 0;
+ } else {
+ zero_bias -= gyro_value;
+ if (zeroing_cycles_left == 0) {
+ // Do all the nice math
+ full_units_offset = zero_bias / kZeroingCycles;
+ remainder_offset = zero_bias % kZeroingCycles;
+ if (remainder_offset < 0) {
+ remainder_offset += kZeroingCycles;
+ --full_units_offset;
+ }
+ }
+ }
+ } else {
+ vParTestSetLED(2, 0);
+
+ int64_t new_angle = gyro_output.angle;
+ if (!bad_reading) new_angle += gyro_value + full_units_offset;
+ if (remainder_sum >= kZeroingCycles) {
+ remainder_sum -= kZeroingCycles;
+ new_angle += 1;
+ }
+ portENTER_CRITICAL();
+ gyro_output.angle = new_angle;
+ gyro_output.last_reading_bad = bad_reading;
+ portEXIT_CRITICAL();
+ remainder_sum += remainder_offset;
+ }
+ }
+}
+
+void gyro_init(void) {
+ gyro_output.initialized = 0;
+
+ xTaskCreate(gyro_read_task, (signed char *) "gyro",
+ configMINIMAL_STACK_SIZE + 100, NULL,
+ tskIDLE_PRIORITY + 2, NULL);
+}
diff --git a/gyro_board/src/usb/gyro.h b/gyro_board/src/usb/gyro.h
new file mode 100644
index 0000000..2c74aad
--- /dev/null
+++ b/gyro_board/src/usb/gyro.h
@@ -0,0 +1,20 @@
+#ifndef GYRO_BOARD_SRC_USB_GYRO_H_
+#define GYRO_BOARD_SRC_USB_GYRO_H_
+
+#include <stdint.h>
+
+// Does everything to set up the gyro code, including starting a task which
+// reads and integrates the gyro values and blinks the LEDs etc.
+void gyro_init(void);
+
+struct GyroOutput {
+ int64_t angle;
+ int last_reading_bad;
+ int gyro_bad;
+ int initialized;
+};
+// This gets updated in a portENTER_CRITICAL/portEXIT_CRITICAL() block so all of
+// the values will be in sync.
+extern struct GyroOutput gyro_output;
+
+#endif // GYRO_BOARD_SRC_USB_GYRO_H_
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 6e69970..3ac21c8 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -31,378 +31,190 @@
/* Demo app includes. */
#include "flash.h"
#include "partest.h"
-#include "analog.h"
-#include "spi.h"
#include "LPCUSB/usbapi.h"
-/*-----------------------------------------------------------*/
-
-/* The time between cycles of the 'check' functionality (defined within the
-tick hook. */
-#define mainCHECK_DELAY ((portTickType) 5000 / portTICK_RATE_MS)
-
-/* Task priorities. */
-#define mainQUEUE_POLL_PRIORITY (tskIDLE_PRIORITY + 2)
-#define mainSEM_TEST_PRIORITY (tskIDLE_PRIORITY + 1)
-#define mainBLOCK_Q_PRIORITY (tskIDLE_PRIORITY + 2)
-#define mainUIP_TASK_PRIORITY (tskIDLE_PRIORITY + 3)
-#define mainINTEGER_TASK_PRIORITY (tskIDLE_PRIORITY)
-#define mainGEN_QUEUE_TASK_PRIORITY (tskIDLE_PRIORITY)
-#define mainFLASH_TASK_PRIORITY (tskIDLE_PRIORITY + 2)
-
-/* The WEB server has a larger stack as it utilises stack hungry string
-handling library calls. */
-#define mainBASIC_WEB_STACK_SIZE (configMINIMAL_STACK_SIZE * 4)
-
-int32_t goal = 0;
-int64_t gyro_angle = 0;
-
-/*-----------------------------------------------------------*/
-
-/*
- * Configure the hardware for the demo.
- */
-static void prvSetupHardware(void);
-
-/*
- * The task that handles the USB stack.
- */
-extern void vUSBTask(void *pvParameters);
-
-extern int VCOM_getchar(void);
-
-int VCOM_putchar(int c);
-
-inline int32_t encoder()
-{
- return (int32_t)QEI->QEIPOS;
-}
-
-static portTASK_FUNCTION(vPrintPeriodic, pvParameters)
-{
- portTickType xLastFlashTime;
-
- /* We need to initialise xLastFlashTime prior to the first call to
- vTaskDelayUntil(). */
- xLastFlashTime = xTaskGetTickCount();
-
- analog_init();
-
- encoder_init();
-
- // Wait 100 ms for it to boot.
- vTaskDelayUntil(&xLastFlashTime, 100 / portTICK_RATE_MS);
- spi_init();
-
- // Enable USB. The PC has probably disconnected it now.
- USBHwAllowConnect();
-
- // TODO(aschuh): Write this into a gyro calibration function, and check all the outputs.
- vTaskDelayUntil(&xLastFlashTime, 50 / portTICK_RATE_MS);
- enable_gyro_csel();
- printf("SPI Gyro Second Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
- disable_gyro_csel();
-
- vTaskDelayUntil(&xLastFlashTime, 50 / portTICK_RATE_MS);
- enable_gyro_csel();
- printf("SPI Gyro Third Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
- disable_gyro_csel();
-
- vTaskDelayUntil(&xLastFlashTime, 10 / portTICK_RATE_MS);
- enable_gyro_csel();
- printf("SPI Gyro Fourth Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0000));
- disable_gyro_csel();
- const int hz = 200;
- const int flash_hz = 10;
- const int startup_cycles = hz * 2;
- const int zeroing_cycles = hz * 6;
- int32_t zero_bias = 0;
- int32_t startup_cycles_left = startup_cycles;
- int32_t zeroing_cycles_left = zeroing_cycles;
- int32_t full_units_offset = 0;
- int32_t remainder_offset = 0;
- int32_t remainder_sum = 0;
- int32_t led_flash = 0;
- vParTestSetLED(0, 0);
-
- for (;;) {
- led_flash ++;
- if (led_flash < hz / flash_hz / 2) {
- vParTestSetLED(1, 0);
- } else {
- vParTestSetLED(1, 1);
- }
- if (led_flash >= hz / flash_hz) {
- led_flash = 0;
- }
- /* Delay for half the flash period then turn the LED on. */
- vTaskDelayUntil(&xLastFlashTime, 1000 / hz / portTICK_RATE_MS);
- enable_gyro_csel();
- uint16_t high_value = transfer_spi_bytes(0x2000);
- uint16_t low_value = transfer_spi_bytes(0x0000);
- disable_gyro_csel();
- int16_t gyro_value = -((int16_t)((((uint32_t)high_value << 16) | (uint32_t)low_value) >> 10));
-
- if (startup_cycles_left) {
- vParTestSetLED(2, 0);
- --startup_cycles_left;
- } else if (zeroing_cycles_left) {
- vParTestSetLED(2, 1);
- //printf("Zeroing ");
- --zeroing_cycles_left;
- zero_bias -= gyro_value;
- if (zeroing_cycles_left == 0) {
- // Do all the nice math
- full_units_offset = zero_bias / zeroing_cycles;
- remainder_offset = zero_bias % zeroing_cycles;
- if (remainder_offset < 0) {
- remainder_offset += zeroing_cycles;
- --full_units_offset;
- }
- }
- } else {
- vParTestSetLED(2, 0);
- int64_t new_angle = gyro_angle + gyro_value + full_units_offset;
- if (remainder_sum >= zeroing_cycles) {
- remainder_sum -= zeroing_cycles;
- new_angle += 1;
- }
- NVIC_DisableIRQ(USB_IRQn);
- gyro_angle = new_angle;
- NVIC_EnableIRQ(USB_IRQn);
- remainder_sum += remainder_offset;
- }
- //printf("Angle %d Rate %d\n", (int)(gyro_angle / 16), (int)(gyro_value + full_units_offset));
-
- //printf("time: %d analog %d encoder %d goal %d\n", (int)i, (int)analog(5), (int)encoder(), (int)goal);
-
- //printf("time: %d encoder %d goal %d\n", (int)i, (int)encoder(), (int)goal);
- /*
- for(i = 0; i < 4; i++){
- printf("analog(%d) => %d\n",i,analog(i));
- }
- for(i = 1; i < 13; i++){
- printf("digital(%d) => %d\n",i,digital(i));
- }
- for(i = 0; i < 4; i++){
- printf("dip(%d) => %d\n",i,dip(i));
- }
- for(i = 0; i < 4; i++){
- printf("encoder(%d) => %d\n",i,encoder_bits(i));
- }
- for(i = 0; i < 4; i++){
- printf("encoder_val(%d) => %d\n",i,(int)encoder_val(i));
- }*/
- }
-}
-
-static portTASK_FUNCTION(vSensorPoll, pvParameters)
-{
- portTickType xLastFlashTime;
- xLastFlashTime = xTaskGetTickCount();
-
-
- vTaskDelayUntil(&xLastFlashTime, 1000 / portTICK_RATE_MS);
-
- for(;;){
-
- vTaskDelayUntil(&xLastFlashTime, 20 / portTICK_RATE_MS);
- /*
- printf("not realtime! e0:%d e1:%d e2:%d e3:%d e4:%d e5:%d angle:%d \n", (int)encoder_val(0),
- (int)encoder_val(1),
- (int)encoder_val(2),
- (int)encoder_val(3),
- (int)encoder_val(4),
- (int)encoder_val(5),
- (int)gyro_angle);
-*/
- }
-}
-
-
+#include "analog.h"
+#include "digital.h"
+#include "encoder.h"
#include "CAN.h"
+#include "gyro.h"
+extern void usb_init(void);
-/*-----------------------------------------------------------*/
+// Sets up (and connects) PLL0.
+// The CPU will be running at 100 MHz with a 12 MHz clock input when this is
+// done.
+static void setup_PLL0(void) {
+ // If PLL0 is currently connected.
+ if (SC->PLL0STAT & (1 << 25)) {
+ /* Enable PLL0, disconnected. */
+ SC->PLL0CON = 1;
+ SC->PLL0FEED = PLLFEED_FEED1;
+ SC->PLL0FEED = PLLFEED_FEED2;
+ }
-int main(void)
-{
- // Configure the hardware
- prvSetupHardware();
+ // Disable PLL0, disconnected.
+ SC->PLL0CON = 0;
+ SC->PLL0FEED = PLLFEED_FEED1;
+ SC->PLL0FEED = PLLFEED_FEED2;
- /* Start the standard demo tasks. These are just here to exercise the
- kernel port and provide examples of how the FreeRTOS API can be used. */
- //vStartLEDFlashTasks(mainFLASH_TASK_PRIORITY);
+ // Enable main OSC.
+ SC->SCS |= 1 << 5;
+ // Wait until it's ready.
+ while (!(SC->SCS & (1 << 6)));
- /* Create the USB task. */
- xTaskCreate(vUSBTask, (signed char *) "USB", configMINIMAL_STACK_SIZE + 1020, (void *) NULL, tskIDLE_PRIORITY + 3, NULL);
+ // Select main OSC as the PLL0 clock source.
+ SC->CLKSRCSEL = 0x1;
- xTaskCreate(vPrintPeriodic, (signed char *) "PRINTx", configMINIMAL_STACK_SIZE + 100, NULL, tskIDLE_PRIORITY + 2, NULL);
+ // Set up PLL0 to output 400MHz.
+ // 12MHz * 50 / 3 * 2 = 400MHz.
+ // The input is 12MHz (from the crystal), M = 50, and N = 3.
+ SC->PLL0CFG = 0x20031;
+ SC->PLL0FEED = PLLFEED_FEED1;
+ SC->PLL0FEED = PLLFEED_FEED2;
+ // Enable PLL0, disconnected.
+ SC->PLL0CON = 1;
+ SC->PLL0FEED = PLLFEED_FEED1;
+ SC->PLL0FEED = PLLFEED_FEED2;
- xTaskCreate(vSensorPoll, (signed char *) "SENSORs", configMINIMAL_STACK_SIZE + 100, NULL, tskIDLE_PRIORITY + 1, NULL);
+ // Set clock divider to dividing by 4 to give a final frequency of 100MHz.
+ SC->CCLKCFG = 0x03;
- initCAN();
+ // Configure flash accelerator to use 5 CPU clocks like the datasheet says for
+ // a 100MHz clock.
+ SC->FLASHCFG = 0x403a;
- // Start the scheduler.
- vTaskStartScheduler();
+ // Wait until PLL0 is locked.
+ while (((SC->PLL0STAT & (1 << 26)) == 0));
- /* Will only get here if there was insufficient memory to create the idle
- task. The idle task is created within vTaskStartScheduler(). */
- for (;;);
+ // Enable PLL0 and connect.
+ SC->PLL0CON = 3;
+ SC->PLL0FEED = PLLFEED_FEED1;
+ SC->PLL0FEED = PLLFEED_FEED2;
+
+ // Wait until PLL0 is connected.
+ while (((SC->PLL0STAT & (1 << 25)) == 0));
}
-/*-----------------------------------------------------------*/
+// Configures PLL1 as the clock for USB.
+static void setup_PLL1(void) {
+ // If PLL1 is currently connected.
+ if (SC->PLL1STAT & (1 << 9)) {
+ // Enable PLL1, disconnected.
+ SC->PLL1CON = 1;
+ SC->PLL1FEED = PLLFEED_FEED1;
+ SC->PLL1FEED = PLLFEED_FEED2;
+ }
-void vApplicationTickHook(void)
-{
- static unsigned long ulTicksSinceLastDisplay = 0;
+ // Disable PLL1, disconnected.
+ SC->PLL1CON = 0;
+ SC->PLL1FEED = PLLFEED_FEED1;
+ SC->PLL1FEED = PLLFEED_FEED2;
- /* Called from every tick interrupt as described in the comments at the top
- of this file.
+ // Set up PLL1 to produce the required 48MHz from the crystal.
+ // USBCLK = 12MHz * 4 = 48MHz.
+ // FCCO = USBCLK * 2 * 3 = 288MHz.
+ // The input is 12MHz, M = 4, and P = 3.
+ SC->PLL1CFG = 0x23;
+ SC->PLL1FEED = PLLFEED_FEED1;
+ SC->PLL1FEED = PLLFEED_FEED2;
- Have enough ticks passed to make it time to perform our health status
- check again? */
+ /* Enable PLL1, disconnected. */
+ SC->PLL1CON = 1;
+ SC->PLL1FEED = PLLFEED_FEED1;
+ SC->PLL1FEED = PLLFEED_FEED2;
- ulTicksSinceLastDisplay++;
+ // Wait until PLL1 is locked.
+ while (((SC->PLL1STAT & (1 << 10)) == 0));
- if (ulTicksSinceLastDisplay >= mainCHECK_DELAY) {
- /* Reset the counter so these checks run again in mainCHECK_DELAY
- ticks time. */
- ulTicksSinceLastDisplay = 0;
- }
+ /* Enable PLL1 and connect it. */
+ SC->PLL1CON = 3;
+ SC->PLL1FEED = PLLFEED_FEED1;
+ SC->PLL1FEED = PLLFEED_FEED2;
+
+ // Wait until PLL1 is connected.
+ while (((SC->PLL1STAT & (1 << 9)) == 0));
}
-/*-----------------------------------------------------------*/
-void prvSetupHardware(void)
-{
- // Setup the peripherals.
- // The CPU will be running at 100 MHz with a 12 MHz clock input.
+// Setup the peripherals.
+static void setup_hardware(void) {
+ // Setup GPIO power.
+ SC->PCONP = PCONP_PCGPIO;
- // Setup GPIO power.
- SC->PCONP = PCONP_PCGPIO;
- // Disable TPIU.
- PINCON->PINSEL10 = 0;
+ // Disable TPIU.
+ PINCON->PINSEL10 = 0;
- // Setup PLL0 so that the CPU runs at 100 MHz.
- if (SC->PLL0STAT & (1 << 25)) {
- /* Enable PLL, disconnected. */
- SC->PLL0CON = 1;
- SC->PLL0FEED = PLLFEED_FEED1;
- SC->PLL0FEED = PLLFEED_FEED2;
- }
+ setup_PLL0();
- // Disable PLL, disconnected.
- SC->PLL0CON = 0;
- SC->PLL0FEED = PLLFEED_FEED1;
- SC->PLL0FEED = PLLFEED_FEED2;
+ setup_PLL1();
- // Enable main OSC and wait until it's ready.
- SC->SCS |= 0x20;
- while (!(SC->SCS & 0x40));
+ // Set CAN to run at CCLK/6, which should have it running about 1 Mbit (1.042)
+ SC->PCLKSEL0 = 0xff555555;
- // select main OSC, 12MHz, as the PLL clock source.
- SC->CLKSRCSEL = 0x1;
-
- SC->PLL0CFG = 0x20031;
- SC->PLL0FEED = PLLFEED_FEED1;
- SC->PLL0FEED = PLLFEED_FEED2;
-
- // Enable PLL, disconnected.
- SC->PLL0CON = 1;
- SC->PLL0FEED = PLLFEED_FEED1;
- SC->PLL0FEED = PLLFEED_FEED2;
-
- // Set clock divider.
- SC->CCLKCFG = 0x03;
-
- // Configure flash accelerator.
- SC->FLASHCFG = 0x403a;
-
- // Check lock bit status.
- while (((SC->PLL0STAT & (1 << 26)) == 0));
-
- // Enable and connect.
- SC->PLL0CON = 3;
- SC->PLL0FEED = PLLFEED_FEED1;
- SC->PLL0FEED = PLLFEED_FEED2;
-
- while (((SC->PLL0STAT & (1 << 25)) == 0));
-
- // Configure the clock for the USB.
- if (SC->PLL1STAT & (1 << 9)) {
- // Enable PLL, disconnected.
- SC->PLL1CON = 1;
- SC->PLL1FEED = PLLFEED_FEED1;
- SC->PLL1FEED = PLLFEED_FEED2;
- }
-
- // Disable PLL, disconnected.
- SC->PLL1CON = 0;
- SC->PLL1FEED = PLLFEED_FEED1;
- SC->PLL1FEED = PLLFEED_FEED2;
-
- SC->PLL1CFG = 0x23;
- SC->PLL1FEED = PLLFEED_FEED1;
- SC->PLL1FEED = PLLFEED_FEED2;
-
- /* Enable PLL, disconnected. */
- SC->PLL1CON = 1;
- SC->PLL1FEED = PLLFEED_FEED1;
- SC->PLL1FEED = PLLFEED_FEED2;
- while (((SC->PLL1STAT & (1 << 10)) == 0));
-
- /* Enable and connect. */
- SC->PLL1CON = 3;
- SC->PLL1FEED = PLLFEED_FEED1;
- SC->PLL1FEED = PLLFEED_FEED2;
- while (((SC->PLL1STAT & (1 << 9)) == 0));
-
- // Setup the peripheral bus to be the same as the CCLK, 100 MHz.
- // Set CAN to run at CCLK/6, which should have it running about 1 Mbit (1.042)
- SC->PCLKSEL0 = 0xff555555;
-
- /* Configure the LEDs. */
- vParTestInitialise();
+ /* Configure the LEDs. */
+ vParTestInitialise();
}
-/*-----------------------------------------------------------*/
+
+int main(void) {
+ setup_hardware();
+
+ digital_init();
+
+ analog_init();
+
+ encoder_init();
+
+ gyro_init();
+
+ initCAN();
+
+ usb_init();
+
+ // Start the scheduler.
+ vTaskStartScheduler();
+
+ /* Will only get here if there was insufficient memory to create the idle
+ task. The idle task is created within vTaskStartScheduler(). */
+ for (;;) {}
+}
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed char *pcTaskName)
{
- /* This function will get called if a task overflows its stack. */
+ /* This function will get called if a task overflows its stack. */
- (void) pxTask;
- (void) pcTaskName;
+ (void) pxTask;
+ (void) pcTaskName;
- for (;;);
-}
-/*-----------------------------------------------------------*/
-
-void vConfigureTimerForRunTimeStats(void)
-{
- const unsigned long TCR_COUNT_RESET = 2, CTCR_CTM_TIMER = 0x00, TCR_COUNT_ENABLE = 0x01;
-
- /* This function configures a timer that is used as the time base when
- collecting run time statistical information - basically the percentage
- of CPU time that each task is utilising. It is called automatically when
- the scheduler is started (assuming configGENERATE_RUN_TIME_STATS is set
- to 1). */
-
- /* Power up and feed the timer. */
- SC->PCONP |= 0x02UL;
- SC->PCLKSEL0 = (SC->PCLKSEL0 & (~(0x3 << 2))) | (0x01 << 2);
-
- /* Reset Timer 0 */
- TIM0->TCR = TCR_COUNT_RESET;
-
- /* Just count up. */
- TIM0->CTCR = CTCR_CTM_TIMER;
-
- /* Prescale to a frequency that is good enough to get a decent resolution,
- but not too fast so as to overflow all the time. */
- TIM0->PR = (configCPU_CLOCK_HZ / 10000UL) - 1UL;
-
- /* Start the counter. */
- TIM0->TCR = TCR_COUNT_ENABLE;
+ for (;;);
}
+// This is what portCONFIGURE_TIMER_FOR_RUN_TIME_STATS in FreeRTOSConfig.h
+// actually calls.
+// It sets up timer 0 to use for timing.
+void vConfigureTimerForRunTimeStats(void) {
+ const unsigned long TCR_COUNT_RESET = 2, CTCR_CTM_TIMER = 0x00, TCR_COUNT_ENABLE = 0x01;
+
+ /* This function configures a timer that is used as the time base when
+ collecting run time statistical information - basically the percentage
+ of CPU time that each task is utilising. It is called automatically when
+ the scheduler is started (assuming configGENERATE_RUN_TIME_STATS is set
+ to 1). */
+
+ /* Power up and feed the timer. */
+ SC->PCONP |= 0x02UL;
+ SC->PCLKSEL0 = (SC->PCLKSEL0 & (~(0x3 << 2))) | (0x01 << 2);
+
+ /* Reset Timer 0 */
+ TIM0->TCR = TCR_COUNT_RESET;
+
+ /* Just count up. */
+ TIM0->CTCR = CTCR_CTM_TIMER;
+
+ /* Prescale to a frequency that is good enough to get a decent resolution,
+ but not too fast so as to overflow all the time. */
+ TIM0->PR = (configCPU_CLOCK_HZ / 10000UL) - 1UL;
+
+ /* Start the counter. */
+ TIM0->TCR = TCR_COUNT_ENABLE;
+}
diff --git a/gyro_board/src/usb/spi.c b/gyro_board/src/usb/spi.c
deleted file mode 100644
index de41866..0000000
--- a/gyro_board/src/usb/spi.c
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "stdio.h"
-#include "FreeRTOS.h"
-#include "spi.h"
-
-void spi_init (void) {
- SC->PCONP |= PCONP_PCSPI;
- SC->PCLKSEL0 |= 0x00010000;
-
- // Hook up the interrupt
- //NVIC_EnableIRQ(SPI_IRQn);
-
- // SCK
- PINCON->PINSEL0 &= 0x3fffffff;
- PINCON->PINSEL0 |= 0xc0000000;
-
- // SSEL, MISO, MOSI
- // SSEL is GPIO, and needs to be done manually.
- disable_gyro_csel();
- GPIO0->FIODIR |= 0x00010000;
- PINCON->PINSEL1 &= 0xffffffc0;
- PINCON->PINSEL1 |= 0x0000003c;
-
- // Master mode, 16 bits/frame, enable interrupts
- SPI->SPCR = 0x000000a4;
- // 13 clocks per cycle. This works out to a 7.7 mhz buss.
- SPI->SPCCR = 0x0000000d;
-
- // TODO(aschuh): Implement the gyro bring-up blocking first.
- // Then use interrupts.
- enable_gyro_csel();
- printf("SPI Gyro Initial Response 0x%x %x\n", transfer_spi_bytes(0x2000), transfer_spi_bytes(0x0003));
- disable_gyro_csel();
-}
-
-// TODO: DMA? SSP0? SSP0 should have a buffer, which would be very nice.
-uint16_t transfer_spi_bytes(uint16_t data) {
- SPI->SPDR = (uint32_t)data;
- while (!(SPI->SPSR & 0x80));
- return SPI->SPDR;
-}
-
-void disable_gyro_csel (void) {
- // Set the CSEL pin high to deselect it.
- GPIO0->FIOSET = 0x00010000;
-}
-
-void enable_gyro_csel (void) {
- // Clear the CSEL pin high to select it.
- GPIO0->FIOCLR = 0x00010000;
-}
-
-void SPI_IRQHandler(void) {
- int status = SPI->SPSR;
- if (status & 0x80) {
- // Transfer completed.
- }
-
- // Clear the interrupt?
- SPI->SPINT = 0x00000001;
-}
diff --git a/gyro_board/src/usb/spi.h b/gyro_board/src/usb/spi.h
deleted file mode 100644
index 6dbc511..0000000
--- a/gyro_board/src/usb/spi.h
+++ /dev/null
@@ -1,9 +0,0 @@
-#ifndef __SPI_H__
-#define __SPI_H__
-
-void spi_init (void);
-uint16_t transfer_spi_bytes(uint16_t data);
-void disable_gyro_csel (void);
-void enable_gyro_csel (void);
-
-#endif // __SPI_H__
diff --git a/gyro_board/src/usb/toolchain-build-notes.txt b/gyro_board/src/usb/toolchain-build-notes.txt
index 700a1f3..d81c905 100644
--- a/gyro_board/src/usb/toolchain-build-notes.txt
+++ b/gyro_board/src/usb/toolchain-build-notes.txt
@@ -3,7 +3,7 @@
The flags are from logic, Austin's gcc, and
<http://www.coactionos.com/getting-started/43-building-and-installing-a-cortex-m3-compiler-on-ubuntu.html>
(for some of the newlib flags).
-TODO(brians): make sure that it generates code that works and then update this
+This has been uploaded to our package repository for wheezy as arm-eabi-gcc.
binutils
../binutils-2.23.2/configure --prefix=/opt/cortex-m3 --target=arm-eabi