int http_connection_t::on_headers_complete() {
/*
if( state == STATE_HEADER_VALUE ) {
serial_write_buffer(buffer.data(), buffer.length());
serial_write_line();
}
*/
state = STATE_HEADERS_COMPLETE;
serial_write_line("<headers/>");
return 0;
}
int fc_init(void)
{
if (!is_initialized)
{
is_initialized = 1;
char *serial_port;
opcd_param_t params[] =
{
{"serial_port", &serial_port},
{"setting", &setting},
OPCD_PARAMS_END
};
opcd_params_apply("actuators.mk_fc.", params);
int ret;
/* perform initialization once here: */
if ((ret = serial_open(&port, serial_port, 57600, ICRNL, ICANON, 0)) != 0)
{
return ret;
}
else
{
const struct timespec dreq_period = {0, DREQ_THREAD_TIMEOUT_MS * NSEC_PER_MSEC};
frame_t frame;
char data = 1;
LOG(LL_INFO, "setting up mikrokopter interface");
build_frame(frame, OUT_NC_REDIRECT_UART, (const plchar_t *)&data, 0);
serial_write_line(&port, frame);
rpm = malloc(sizeof(float) * platform_motors());
for (int i = 0; i < platform_motors(); i++)
{
rpm[i] = 0.0f;
}
periodic_thread_start(&dreq_thread, dreq_thread_func, DREQ_THREAD_NAME,
DREQ_THREAD_PRIORITY, dreq_period, NULL);
simple_thread_start(&sread_thread, sread_thread_func, SREAD_THREAD_NAME,
SREAD_THREAD_PRIORITY, NULL);
LOG(LL_INFO, "mikrokopter interface up and running");
}
}
else
{
LOG(LL_DEBUG, "mikrokopter interface already running");
}
return 0;
}
SIMPLE_THREAD_END
PERIODIC_THREAD_BEGIN(dreq_thread_func)
{
const char interval = 10;
frame_t frame;
PERIODIC_THREAD_LOOP_BEGIN
{
build_frame(frame, OUT_FC_DEBUG, (const plchar_t *)&interval, 1);
serial_write_line(&port, frame);
}
PERIODIC_THREAD_LOOP_END
}
PERIODIC_THREAD_END
int fc_write_motors(mixer_in_t *data)
{
struct ExternControl ext_ctrl;
frame_t frame;
bzero(&ext_ctrl, sizeof(struct ExternControl));
ext_ctrl.gas = 255.0 * data->gas;
ext_ctrl.pitch = data->pitch * 127.0;
ext_ctrl.roll = data->roll * 127.0;
ext_ctrl.yaw = data->yaw * 127.0;
ext_ctrl.config = 1;
build_frame(frame, OUT_FC_EXTERN_CONTROL,
(const plchar_t *)&ext_ctrl,
sizeof(struct ExternControl));
return serial_write_line(&port, frame);
}
static int motors_action(char action, char setting)
{
frame_t frame;
char data[2];
if (action == 'e') /* start motors */
{
data[0] = action;
data[1] = setting;
build_frame(frame, OUT_FC_MOTORS_ACTION,
(const plchar_t *)data, 2);
}
else /* stop motors */
{
data[0] = 'd';
build_frame(frame, OUT_FC_MOTORS_ACTION,
(const plchar_t *)data, 1);
}
int result;
do
{
serial_write_line(&port, frame);
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
ts = timespec_add_ms(ts, 100);
pthread_mutex_lock(&ack_mutex);
result = pthread_cond_timedwait(&ack, &ack_mutex, &ts);
pthread_mutex_unlock(&ack_mutex);
}
while (result == ETIMEDOUT);
return 0;
}
int http_connection_t::on_status_complete() {
state = STATE_STATUS_COMPLETE;
serial_write_line("<status/>");
return 0;
}
int http_connection_t::on_url(const char* const at, const size_t length) {
serial_write_line("<url/>");
return 0;
}
int http_connection_t::on_message_begin() {
state = STATE_MESSAGE_BEGIN;
serial_write_line("<message>");
return 0;
}
int http_connection_t::on_message_complete() {
state = STATE_MESSAGE_COMPLETE;
serial_write_line("</message>");
return 0;
}
/*
* Receive IPMI response from the device
* Len: buffer size
* Returns: -1 or response lenth on success
*/
static int
recv_response(struct ipmi_intf * intf, unsigned char *data, int len)
{
char hex_rs[IPMI_SERIAL_MAX_RESPONSE * 3];
int i, j, resp_len = 0;
long rv;
char *p, *pp;
char ch, str_hex[3];
p = hex_rs;
while (1) {
if ((rv = serial_read_line(intf, p, sizeof(hex_rs) - resp_len)) < 0) {
/* error */
return -1;
}
p += rv;
resp_len += rv;
if (*(p - 2) == ']' && (*(p - 1) == '\n' || *(p - 1) == '\r')) {
*p = 0;
break;
}
}
p = strrchr(hex_rs, '[');
if (!p) {
lprintf(LOG_ERR, "Serial response is invalid");
return -1;
}
p++;
pp = strchr(p, ']');
if (!pp) {
lprintf(LOG_ERR, "Serial response is invalid");
return -1;
}
*pp = 0;
/* was it an error? */
if (strncmp(p, "ERR ", 4) == 0) {
serial_write_line(intf, "\r\r\r\r");
sleep(1);
serial_flush(intf);
errno = 0;
rv = strtoul(p + 4, &p, 16);
if ((rv && rv < 0x100 && *p == '\0')
|| (rv == 0 && !errno)) {
/* The message didn't get it through. The upper
level will have to re-send */
return 0;
} else {
lprintf(LOG_ERR, "Serial response is invalid");
return -1;
}
}
/* this is needed for correct string to long conversion */
str_hex[2] = 0;
/* parse the response */
i = 0;
j = 0;
while (*p) {
if (i >= len) {
lprintf(LOG_ERR, "Serial response is too long(%d, %d)", i, len);
return -1;
}
ch = *(p++);
if (isxdigit(ch)) {
str_hex[j++] = ch;
} else {
if (j == 1 || !isspace(ch)) {
lprintf(LOG_ERR, "Serial response is invalid");
return -1;
}
}
if (j == 2) {
unsigned long tmp;
errno = 0;
/* parse the hex number */
tmp = strtoul(str_hex, NULL, 16);
if ( tmp > 0xFF || ( !tmp && errno ) ) {
lprintf(LOG_ERR, "Serial response is invalid");
return -1;
}
data[i++] = tmp;
j = 0;
}
}
return i;
}
