#include "rc_proto.h"
#include "xbee_user.h"
#include "spi_servo.h"
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
#include "main.h"
#define N_SERVO 6
.send_servo_min_ms = 50,
.send_servo_max_ms = 300,
.send_power_probe_ms = 500,
+ .send_gps_pos_ms = 500,
+ .send_imu_pos_ms = 100,
.autobypass_ms = 500,
};
uint32_t servo_tx;
uint32_t stats_rx;
uint32_t stats_tx;
+ uint32_t gps_pos_rx;
+ uint32_t gps_pos_tx;
+ uint32_t imu_pos_rx;
+ uint32_t imu_pos_tx;
uint32_t echo_ans_latency_sum;
};
static struct rc_proto_stats_data stats; /* local stats */
/* define tx mode (disabled, send from spi, bypass), rx mode (auto-bypass),
... */
-static uint8_t rc_proto_flags;
+static uint16_t rc_proto_flags;
/* callout managing rc_proto (ex: sending of servos periodically) */
static struct callout rc_proto_timer;
* the previous PL value (0 or 4) so we can alternate. */
int8_t power_level_global = -1;
+#define POW_IMU_GPS 4
+
/* update power level when we receive the answer from DB. The request is sent by
* rc_proto_rx_power_probe(). */
static int8_t update_power_level(int8_t retcode, void *frame, unsigned len,
return ret;
}
-void rc_proto_set_mode(uint8_t flags)
+/* send a gps_pos message: no answer expected */
+int8_t rc_proto_send_gps_pos(uint64_t addr, int8_t power)
+{
+ struct rc_proto_gps_pos gps_msg;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+ uint8_t irq_flags;
+
+ gps_msg.type = RC_PROTO_GPS_POS;
+
+ IRQ_LOCK(irq_flags);
+ gps_msg.valid = !!(imuboard_status.flags & I2C_IMUBOARD_STATUS_GPS_OK);
+ gps_msg.latitude = imuboard_status.latitude;
+ gps_msg.longitude = imuboard_status.longitude;
+ gps_msg.altitude = imuboard_status.altitude;
+ IRQ_UNLOCK(irq_flags);
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &gps_msg;
+ msg.iov[0].len = sizeof(gps_msg);
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we also need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.gps_pos_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send a imu_pos message: no answer expected */
+int8_t rc_proto_send_imu_pos(uint64_t addr, int8_t power)
+{
+ struct rc_proto_imu_pos imu_msg;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+ uint8_t irq_flags;
+
+ imu_msg.type = RC_PROTO_IMU_POS;
+
+ IRQ_LOCK(irq_flags);
+ imu_msg.valid = !!(imuboard_status.flags & I2C_IMUBOARD_STATUS_IMU_OK);
+ imu_msg.roll = imuboard_status.roll;
+ imu_msg.pitch = imuboard_status.pitch;
+ imu_msg.yaw = imuboard_status.yaw;
+ IRQ_UNLOCK(irq_flags);
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &imu_msg;
+ msg.iov[0].len = sizeof(imu_msg);
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we also need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.imu_pos_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+
+
+void rc_proto_set_mode(uint16_t flags)
{
rc_proto_flags = flags;
}
-uint8_t rc_proto_get_mode(void)
+uint16_t rc_proto_get_mode(void)
{
return rc_proto_flags;
}
struct rc_proto_hello *rch =
(struct rc_proto_hello *) recvframe->data;
- NOTICE(E_USER_XBEE, "recv hello len=%d", rch->datalen);
+ NOTICE(E_USER_RC_PROTO, "recv hello len=%d", rch->datalen);
stats.hello_rx++;
return 0;
}
(struct rc_proto_echo_req *) recvframe->data;
int8_t power = rce->power;
- NOTICE(E_USER_XBEE, "recv echo len=%d", rce->datalen);
+ NOTICE(E_USER_RC_PROTO, "recv echo len=%d", rce->datalen);
stats.echo_req_rx++;
if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
(struct rc_proto_echo_ans *) recvframe->data;
uint16_t diff;
- NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
+ NOTICE(E_USER_RC_PROTO, "recv echo_ans len=%d", rce->datalen);
stats.echo_ans_rx++;
diff = get_time_ms() - rce->timestamp;
stats.echo_ans_latency_sum += diff;
struct rc_proto_power_probe *rcpb =
(struct rc_proto_power_probe *) recvframe->data;
- NOTICE(E_USER_XBEE, "recv power_probe");
+ NOTICE(E_USER_RC_PROTO, "recv power_probe");
if (datalen != sizeof(*rcpb))
return -1;
struct rc_proto_ack *rca =
(struct rc_proto_ack *) recvframe->data;
- NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
+ NOTICE(E_USER_RC_PROTO, "recv ack, ack=%d", rca->seq);
stats.ack_rx++;
return 0;
}
struct rc_proto_servo *rcs =
(struct rc_proto_servo *) recvframe->data;
- NOTICE(E_USER_XBEE, "recv servo");
+ NOTICE(E_USER_RC_PROTO, "recv servo");
if (datalen != RC_PROTO_SERVO_LEN)
return -1;
struct rc_proto_stats *rcs =
(struct rc_proto_stats *) recvframe->data;
- NOTICE(E_USER_XBEE, "recv stats");
+ NOTICE(E_USER_RC_PROTO, "recv stats");
if (datalen != sizeof(*rcs) + sizeof(peer_stats))
return -1;
return 0;
}
+ /* received by the radio controller */
+ case RC_PROTO_GPS_POS: {
+ struct rc_proto_gps_pos *rcmsg =
+ (struct rc_proto_gps_pos *) recvframe->data;
+
+ if (datalen != sizeof(*rcmsg))
+ return -1;
+
+ stats.gps_pos_rx++;
+ NOTICE(E_USER_RC_PROTO, "GPS received valid=%d %"PRIu32" %"
+ PRIu32" %"PRIu32,
+ rcmsg->valid, rcmsg->latitude, rcmsg->longitude,
+ rcmsg->altitude);
+ return 0;
+ }
+
+ /* received by the radio controller */
+ case RC_PROTO_IMU_POS: {
+ struct rc_proto_imu_pos *rcmsg =
+ (struct rc_proto_imu_pos *) recvframe->data;
+
+ if (datalen != sizeof(*rcmsg))
+ return -1;
+
+ stats.imu_pos_rx++;
+ NOTICE(E_USER_RC_PROTO,
+ "IMU received %d %d %d",
+ rcmsg->roll, rcmsg->pitch, rcmsg->yaw);
+ return 0;
+ }
+
default:
return -1;
}
static uint16_t prev_stats_send;
static uint16_t prev_compute_pow;
static uint16_t prev_power_probe;
+ static uint16_t prev_gps_pos;
+ static uint16_t prev_imu_pos;
static uint8_t pow_probe;
uint16_t t, diff;
}
}
+ /* send gps periodically */
+ if (rc_proto_flags & RC_PROTO_FLAGS_TX_GPS_POS) {
+ diff = t - prev_gps_pos;
+ if (diff > rc_proto_timers.send_gps_pos_ms) {
+ rc_proto_send_gps_pos(rc_proto_dstaddr, POW_IMU_GPS);
+ prev_gps_pos = t;
+ }
+ }
+
+ /* send imu periodically */
+ if (rc_proto_flags & RC_PROTO_FLAGS_TX_IMU_POS) {
+ diff = t - prev_imu_pos;
+ if (diff > rc_proto_timers.send_imu_pos_ms) {
+ rc_proto_send_imu_pos(rc_proto_dstaddr, POW_IMU_GPS);
+ prev_imu_pos = t;
+ }
+ }
+
/* on wing, auto bypass servos if no commands since some time */
if (rc_proto_flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) {
diff = t - servo_rx.time;
callout_schedule(cm, tim, 0);
}
-void rc_proto_dump_stats(void)
+static void __dump_stats(struct rc_proto_stats_data *s)
{
- printf_P(PSTR("rc_proto stats LOCAL\r\n"));
- printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), stats.hello_tx);
- printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), stats.hello_rx);
- printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), stats.echo_req_rx);
- printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), stats.echo_req_tx);
- printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), stats.echo_ans_rx);
- printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), stats.echo_ans_tx);
- printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), stats.power_probe_rx);
- printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), stats.power_probe_tx);
- printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), stats.ack_rx);
- printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), stats.ack_tx);
- printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), stats.servo_rx);
- printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), stats.servo_tx);
- printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), stats.stats_rx);
- printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), stats.stats_tx);
- if (stats.echo_ans_rx != 0) {
+ printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), s->hello_tx);
+ printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), s->hello_rx);
+ printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), s->echo_req_rx);
+ printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), s->echo_req_tx);
+ printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), s->echo_ans_rx);
+ printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), s->echo_ans_tx);
+ printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), s->power_probe_rx);
+ printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), s->power_probe_tx);
+ printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), s->ack_rx);
+ printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), s->ack_tx);
+ printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), s->servo_rx);
+ printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), s->servo_tx);
+ printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), s->stats_rx);
+ printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), s->stats_tx);
+ printf_P(PSTR(" gps_pos_rx: %"PRIu32"\r\n"), s->gps_pos_rx);
+ printf_P(PSTR(" gps_pos_tx: %"PRIu32"\r\n"), s->gps_pos_tx);
+ printf_P(PSTR(" imu_pos_rx: %"PRIu32"\r\n"), s->imu_pos_rx);
+ printf_P(PSTR(" imu_pos_tx: %"PRIu32"\r\n"), s->imu_pos_tx);
+ if (s->echo_ans_rx != 0) {
printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"),
- stats.echo_ans_latency_sum / stats.echo_ans_rx);
+ s->echo_ans_latency_sum / s->echo_ans_rx);
}
+}
+
+void rc_proto_dump_stats(void)
+{
+ printf_P(PSTR("rc_proto stats LOCAL\r\n"));
+ __dump_stats(&stats);
printf_P(PSTR("rc_proto stats PEER\r\n"));
- printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), peer_stats.hello_tx);
- printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), peer_stats.hello_rx);
- printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), peer_stats.echo_req_rx);
- printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), peer_stats.echo_req_tx);
- printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), peer_stats.echo_ans_rx);
- printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), peer_stats.echo_ans_tx);
- printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), peer_stats.power_probe_rx);
- printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), peer_stats.power_probe_tx);
- printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), peer_stats.ack_rx);
- printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), peer_stats.ack_tx);
- printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), peer_stats.servo_rx);
- printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), peer_stats.servo_tx);
- printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), peer_stats.stats_rx);
- printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), peer_stats.stats_tx);
- if (peer_stats.echo_ans_rx != 0) {
- printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"),
- peer_stats.echo_ans_latency_sum / peer_stats.echo_ans_rx);
- }
+ __dump_stats(&peer_stats);
}
void rc_proto_reset_stats(void)
git.droids-corp.org / fpv.git / blobdiff