/** Error message.
* Halts the program while continually sending a fixed error message in SBP
* message format to the FTDI USART, in a way that should get the message
* through to the Python console even if it's interrupting another transmission.
*
* \param msg A pointer to an array of chars containing the error message.
*/
void _screaming_death(const char *pos, const char *msg)
{
__asm__("CPSID if;"); /* Disable all interrupts and faults */
#define SPEAKING_MSG_N 222 /* Maximum length of error message */
static char err_msg[SPEAKING_MSG_N] = "ERROR: ";
strncat(err_msg, pos, SPEAKING_MSG_N - 8);
strncat(err_msg, " : ", SPEAKING_MSG_N - strlen(err_msg) - 1);
strncat(err_msg, msg, SPEAKING_MSG_N - strlen(err_msg) - 1);
strncat(err_msg, "\n", SPEAKING_MSG_N - strlen(err_msg) - 1);
u8 len = strlen(err_msg);
static sbp_state_t sbp_state;
sbp_state_init(&sbp_state);
/* Continuously send error message */
#define APPROX_ONE_SEC 200000000
while (1) {
led_toggle(LED_RED);
for (u32 d = 0; d < APPROX_ONE_SEC; d++) {
__asm__("nop");
}
/* TODO: Send to other UARTs? */
sbp_send_message(&sbp_state, SBP_MSG_PRINT_DEP, 0, len, (u8*)err_msg, &fallback_write_ftdi);
}
}
msg_t sbp_thread(void* arg)
{
(void)arg;
chRegSetThreadName("sbp");
sbp_state_init(&sbp_state);
const SerialConfig m2r_port_config = {
.speed = 115200,
};
sdStart(&M2R_PORT, &m2r_port_config);
while (TRUE) {
sbp_process(&sbp_state, &m2r_read);
}
}
END_TEST
START_TEST(test_sbp_send_message)
{
/* TODO: Tests with different write function behaviour. */
sbp_state_t s;
sbp_state_init(&s);
u8 smsg[] = { 0x22, 0x33 };
fail_unless(sbp_send_message(&s, 0x2233, 0x4455, 0, smsg, 0) == SBP_NULL_ERROR,
"sbp_send_message should return an error if write is NULL");
dummy_reset();
fail_unless(sbp_send_message(&s, 0x2233, 0x4455, 1, 0, &dummy_write)
== SBP_NULL_ERROR,
"sbp_send_message should return an error if payload is NULL and len != 0");
dummy_reset();
fail_unless(sbp_send_message(&s, 0x2233, 0x4455, 0, 0, &dummy_write)
== SBP_OK,
"sbp_send_message should return OK if payload is NULL and len == 0");
u8 zero_len_message[] = {0x55, 0x33, 0x22, 0x55, 0x44, 0x00, 0x2C, 0x4C};
fail_unless(memcmp(dummy_buff, zero_len_message, sizeof(zero_len_message))
== 0,
"sbp_send_message encode error for len = 0");
dummy_reset();
sbp_send_message(&s, 0x2233, 0x6677, sizeof(smsg), smsg, &dummy_write);
u8 awesome_message[] = {0x55, 0x33, 0x22, 0x77, 0x66,
0x02, 0x22, 0x33, 0x8A, 0x33};
fail_unless(memcmp(dummy_buff, awesome_message, sizeof(awesome_message))
== 0,
"sbp_send_message encode error for test message");
}
void open_udp_broadcast_socket(udp_broadcast_context *udp_context,
const char *broadcast_hostname,
int broadcast_port)
{
// set up the socket
int sock;
struct sockaddr_in sock_in;
memset(&sock_in, 0, sizeof(sock_in));
// initalize the socket
sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if (sock < 0) {
sbp_log(LOG_ERR, "open_udp_broadcast_socket: socket() failed: %s", strerror(errno));
}
sock_in.sin_addr.s_addr = htonl(INADDR_ANY);
sock_in.sin_port = htons(0);
sock_in.sin_family = AF_INET;
// bind socket, set permissions
int status = bind(sock, (struct sockaddr *)&sock_in, sizeof(sock_in));
if (status != 0) {
sbp_log(LOG_ERR, "bind failed = %d\n", status);
return;
}
int option_value = 1;
status = setsockopt(sock, SOL_SOCKET, SO_BROADCAST, &option_value, sizeof(option_value));
if (status != 0) {
sbp_log(LOG_ERR, "setsockopt failed = %d\n", status);
return;
}
option_value = 1;
status = setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &option_value, sizeof(option_value));
if (status != 0) {
sbp_log(LOG_ERR, "setsockopt failed = %d\n", status);
return;
}
struct sockaddr_storage addr;
struct addrinfo *resolutions = NULL;
if (getaddrinfo(broadcast_hostname, NULL, NULL, &resolutions) != 0) {
sbp_log(LOG_ERR, "address resolution failed");
return;
}
if (resolutions == NULL) {
sbp_log(LOG_ERR, "no addresses returned by name resolution");
return;
}
memcpy(&addr, resolutions->ai_addr, resolutions->ai_addrlen);
if (resolutions->ai_family == AF_INET) {
((struct sockaddr_in *)&addr)->sin_port = htons(broadcast_port);
} else if (resolutions->ai_family == AF_INET6) {
sbp_log(LOG_ERR, "IPv6 is not supported");
return;
} else {
sbp_log(LOG_ERR, "unknown address family returned from name resolution");
return;
}
udp_context->sock = sock;
udp_context->sock_in_len = resolutions->ai_addrlen;
memcpy(&udp_context->sock_in, &addr, resolutions->ai_addrlen);
freeaddrinfo(resolutions);
sbp_state_init(&udp_context->sbp_state);
sbp_state_set_io_context(&udp_context->sbp_state, udp_context);
}
END_TEST
START_TEST(test_callbacks)
{
sbp_state_t s;
sbp_state_init(&s);
/* Start with no callbacks registered. */
sbp_clear_callbacks(&s);
fail_unless(sbp_find_callback(&s, 0x1234) == 0,
"sbp_find_callback should return NULL if no callbacks registered");
fail_unless(sbp_register_callback(&s, 0x2233, &test_callback, 0, 0) == SBP_NULL_ERROR,
"sbp_register_callback should return an error if node is NULL");
/* Add a first callback. */
static sbp_msg_callbacks_node_t n;
int NUMBER = 42;
fail_unless(sbp_register_callback(&s, 0x2233, 0, 0, &n) == SBP_NULL_ERROR,
"sbp_register_callback should return an error if cb is NULL");
fail_unless(sbp_register_callback(&s, 0x2233, &test_callback, &NUMBER, &n) == SBP_OK,
"sbp_register_callback should return success if everything is groovy");
fail_unless(sbp_register_callback(&s, 0x2233, &test_callback, 0, &n)
== SBP_CALLBACK_ERROR,
"sbp_register_callback should return SBP_CALLBACK_ERROR if a callback "
"of the same type is already registered");
fail_unless(sbp_find_callback(&s, 0x1234) == 0,
"sbp_find_callback should return NULL if callback not registered");
fail_unless(sbp_find_callback(&s, 0x2233) == &n,
"sbp_find_callback didn't return the correct callback node pointer");
fail_unless(sbp_find_callback(&s, 0x2233)->context == &NUMBER,
"sbp_find_callback didn't return the correct context pointer");
/* Add a second callback. */
static sbp_msg_callbacks_node_t m;
int NUMBER2 = 84;
fail_unless(sbp_register_callback(&s, 0x1234, &test_callback2, &NUMBER2, &m) == SBP_OK,
"sbp_register_callback should return success if everything is groovy (2)");
fail_unless(sbp_find_callback(&s, 0x2233) == &n,
"sbp_find_callback didn't return the correct callback function pointer (2)");
fail_unless(sbp_find_callback(&s, 0x2233)->context == &NUMBER,
"sbp_find_callback didn't return the correct context pointer");
fail_unless(sbp_find_callback(&s, 0x1234) == &m,
"sbp_find_callback didn't return the correct callback function pointer (3)");
fail_unless(sbp_find_callback(&s, 0x1234)->context == &NUMBER2,
"sbp_find_callback didn't return the correct context pointer");
fail_unless(sbp_register_callback(&s, 0x1234, &test_callback, 0, &n)
== SBP_CALLBACK_ERROR,
"sbp_register_callback should return SBP_CALLBACK_ERROR if a callback "
"of the same type is already registered (2)");
fail_unless(sbp_find_callback(&s, 0x7788) == 0,
"sbp_find_callback should return NULL if callback not registered (2)");
/* Clear all the registered callbacks and check they can no longer be found. */
sbp_clear_callbacks(&s);
fail_unless(sbp_find_callback(&s, 0x1234) == 0,
"sbp_find_callback should return NULL if no callbacks registered (2)");
fail_unless(sbp_find_callback(&s, 0x2233) == 0,
"sbp_find_callback should return NULL if no callbacks registered (3)");
}
int main(int argc, char **argv)
{
int opt;
int result = 0;
sbp_state_t s;
if (argc <= 1) {
usage(argv[0]);
exit(EXIT_FAILURE);
}
while ((opt = getopt(argc, argv, "p:")) != -1) {
switch (opt) {
case 'p':
serial_port_name = (char *)calloc(strlen(optarg) + 1, sizeof(char));
if (!serial_port_name) {
fprintf(stderr, "Cannot allocate memory!\n");
exit(EXIT_FAILURE);
}
strcpy(serial_port_name, optarg);
break;
case 'h':
usage(argv[0]);
exit(EXIT_FAILURE);
}
}
if (!serial_port_name) {
fprintf(stderr, "Please supply the serial port path where the Piksi is " \
"connected!\n");
exit(EXIT_FAILURE);
}
result = sp_get_port_by_name(serial_port_name, &piksi_port);
if (result != SP_OK) {
fprintf(stderr, "Cannot find provided serial port!\n");
exit(EXIT_FAILURE);
}
result = sp_open(piksi_port, SP_MODE_READ);
if (result != SP_OK) {
fprintf(stderr, "Cannot open %s for reading!\n", serial_port_name);
exit(EXIT_FAILURE);
}
setup_port();
sbp_state_init(&s);
#if 0
/* Register a node and callback, and associate them with a specific message ID. */
sbp_register_callback(&s, SBP_MSG_HEARTBEAT, &hearbeat_callback, NULL,
&heartbeat_callback_node);
sbp_register_callback(&s, SBP_MSG_GPS_TIME, &sbp_gps_time_callback,
NULL, &gps_time_node);
sbp_register_callback(&s, SBP_MSG_POS_LLH, &sbp_pos_llh_callback,
NULL, &pos_llh_node);
sbp_register_callback(&s, SBP_MSG_BASELINE_NED, &sbp_baseline_ned_callback,
NULL, &baseline_ned_node);
sbp_register_callback(&s, SBP_MSG_VEL_NED, &sbp_vel_ned_callback,
NULL, &vel_ned_node);
sbp_register_callback(&s, SBP_MSG_DOPS, &sbp_dops_callback,
NULL, &dops_node);
/* Use sprintf to right justify floating point prints. */
char rj[30];
/* Only want 1 call to SH_SendString as semihosting is quite slow.
* sprintf everything to this array and then print using array. */
char str[1000];
int str_i;
#endif
while (1) {
// sbp_process(&s, &piksi_port_read);
sleep(1);
fprintf(stdout, "hello\n");
#if 0
str_i = 0;
memset(str, 0, sizeof(str));
str_i += sprintf(str + str_i, "\n\n\n\n");
/* Print GPS time. */
str_i += sprintf(str + str_i, "GPS Time:\n");
str_i += sprintf(str + str_i, "\tWeek\t\t: %6d\n", (int)gps_time.wn);
sprintf(rj, "%6.2f", ((float)gps_time.tow) / 1e3);
str_i += sprintf(str + str_i, "\tSeconds\t: %9s\n", rj);
str_i += sprintf(str + str_i, "\n");
/* Print absolute position. */
str_i += sprintf(str + str_i, "Absolute Position:\n");
sprintf(rj, "%4.10lf", pos_llh.lat);
str_i += sprintf(str + str_i, "\tLatitude\t: %17s\n", rj);
sprintf(rj, "%4.10lf", pos_llh.lon);
str_i += sprintf(str + str_i, "\tLongitude\t: %17s\n", rj);
sprintf(rj, "%4.10lf", pos_llh.height);
str_i += sprintf(str + str_i, "\tHeight\t: %17s\n", rj);
str_i += sprintf(str + str_i, "\tSatellites\t: %02d\n", pos_llh.n_sats);
str_i += sprintf(str + str_i, "\n");
/* Print NED (North/East/Down) baseline (position vector from base to rover). */
str_i += sprintf(str + str_i, "Baseline (mm):\n");
str_i += sprintf(str + str_i, "\tNorth\t\t: %6d\n", (int)baseline_ned.n);
str_i += sprintf(str + str_i, "\tEast\t\t: %6d\n", (int)baseline_ned.e);
str_i += sprintf(str + str_i, "\tDown\t\t: %6d\n", (int)baseline_ned.d);
str_i += sprintf(str + str_i, "\n");
/* Print NED velocity. */
str_i += sprintf(str + str_i, "Velocity (mm/s):\n");
str_i += sprintf(str + str_i, "\tNorth\t\t: %6d\n", (int)vel_ned.n);
str_i += sprintf(str + str_i, "\tEast\t\t: %6d\n", (int)vel_ned.e);
str_i += sprintf(str + str_i, "\tDown\t\t: %6d\n", (int)vel_ned.d);
str_i += sprintf(str + str_i, "\n");
/* Print Dilution of Precision metrics. */
str_i += sprintf(str + str_i, "Dilution of Precision:\n");
sprintf(rj, "%4.2f", ((float)dops.gdop / 100));
str_i += sprintf(str + str_i, "\tGDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.hdop / 100));
str_i += sprintf(str + str_i, "\tHDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.pdop / 100));
str_i += sprintf(str + str_i, "\tPDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.tdop / 100));
str_i += sprintf(str + str_i, "\tTDOP\t\t: %7s\n", rj);
sprintf(rj, "%4.2f", ((float)dops.vdop / 100));
str_i += sprintf(str + str_i, "\tVDOP\t\t: %7s\n", rj);
str_i += sprintf(str + str_i, "\n");
#endif
}
result = sp_close(piksi_port);
if (result != SP_OK) {
fprintf(stderr, "Cannot close %s properly!\n", serial_port_name);
}
sp_free_port(piksi_port);
free(serial_port_name);
return 0;
}
