2 * Copyright © 2011 Keith Packard <keithp@keithp.com>
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License along
15 * with this program; if not, write to the Free Software Foundation, Inc.,
16 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
20 #include <ao_companion.h>
23 #define ao_spi_slow(b)
24 #define ao_spi_fast(b)
28 #error HAS_COMPANION not set in ao_companion.c
31 #define AO_COMPANION_SPI_SPEED ao_spi_speed(AO_COMPANION_SPI_BUS, 200000)
33 #define COMPANION_SELECT() do { \
34 ao_spi_get_bit(AO_COMPANION_CS_PORT, \
35 AO_COMPANION_CS_PIN, \
36 AO_COMPANION_SPI_BUS, \
37 AO_COMPANION_SPI_SPEED); \
40 #define COMPANION_DESELECT() do { \
41 ao_spi_put_bit(AO_COMPANION_CS_PORT, \
42 AO_COMPANION_CS_PIN, \
43 AO_COMPANION_SPI_BUS); \
46 struct ao_companion_command ao_companion_command;
47 struct ao_companion_setup ao_companion_setup;
49 uint16_t ao_companion_data[AO_COMPANION_MAX_CHANNELS];
50 uint8_t ao_companion_running;
51 uint8_t ao_companion_mutex;
54 ao_companion_send_command(uint8_t command)
56 ao_companion_command.command = command;
57 ao_companion_command.flight_state = ao_flight_state;
58 ao_companion_command.tick = (uint16_t) ao_time();
59 ao_companion_command.serial = ao_serial_number;
60 ao_companion_command.flight = ao_flight_number;
61 ao_companion_command.accel = (int16_t) ao_accel;
62 ao_companion_command.speed = (int16_t) ao_speed;
63 ao_companion_command.height = (int16_t) ao_height;
64 ao_companion_command.motor_number = ao_motor_number;
65 ao_spi_send(&ao_companion_command, sizeof (ao_companion_command), AO_COMPANION_SPI_BUS);
69 ao_companion_get_setup(void)
72 ao_companion_send_command(AO_COMPANION_SETUP);
73 ao_spi_recv(&ao_companion_setup, sizeof (ao_companion_setup), AO_COMPANION_SPI_BUS);
75 return ((int16_t) ao_companion_setup.board_id ==
76 (int16_t) (uint16_t) (~ao_companion_setup.board_id_inverse));
80 ao_companion_get_data(void)
83 ao_companion_send_command(AO_COMPANION_FETCH);
84 ao_mutex_get(&ao_companion_mutex);
85 ao_spi_recv(&ao_companion_data, ao_companion_setup.channels * 2, AO_COMPANION_SPI_BUS);
86 ao_mutex_put(&ao_companion_mutex);
91 ao_companion_notify(void)
94 ao_companion_send_command(AO_COMPANION_NOTIFY);
102 while (!ao_flight_number)
103 ao_sleep(&ao_flight_number);
104 for (i = 0; i < 10; i++) {
105 ao_delay(AO_SEC_TO_TICKS(1));
106 if ((ao_companion_running = ao_companion_get_setup()))
109 while (ao_companion_running) {
110 if (ao_sleep_for(&ao_flight_state, ao_companion_setup.update_period))
111 ao_companion_get_data();
113 ao_companion_notify();
119 ao_companion_status(void)
122 printf("Companion running: %d\n", ao_companion_running);
123 if (!ao_companion_running)
125 printf("device: %d\n"
126 "update period: %d\n"
129 ao_companion_setup.board_id,
130 ao_companion_setup.update_period,
131 ao_companion_setup.channels);
132 for(i = 0; i < ao_companion_setup.channels; i++)
133 printf(" %5u", ao_companion_data[i]);
137 const struct ao_cmds ao_companion_cmds[] = {
138 { ao_companion_status, "L\0Companion link status" },
142 static struct ao_task ao_companion_task;
145 ao_companion_init(void)
147 ao_enable_output(AO_COMPANION_CS_PORT, AO_COMPANION_CS_PIN, 1);
148 ao_cmd_register(&ao_companion_cmds[0]);
149 ao_add_task(&ao_companion_task, ao_companion, "companion");