diff options
38 files changed, 1563 insertions, 92 deletions
diff --git a/altoslib/AltosGPS.java b/altoslib/AltosGPS.java index 8037eb93..57ac4061 100644 --- a/altoslib/AltosGPS.java +++ b/altoslib/AltosGPS.java @@ -92,6 +92,10 @@ public class AltosGPS implements Cloneable { return odt.toEpochSecond(); } + public AltosLatLon lat_lon() { + return new AltosLatLon(lat, lon); + } + public AltosGPS(AltosTelemetryMap map) throws ParseException { String state = map.get_string(AltosTelemetryLegacy.AO_TELEM_GPS_STATE, AltosTelemetryLegacy.AO_TELEM_GPS_STATE_ERROR); diff --git a/altoslib/AltosMap.java b/altoslib/AltosMap.java index b033cbff..69142c37 100644 --- a/altoslib/AltosMap.java +++ b/altoslib/AltosMap.java @@ -222,8 +222,7 @@ public class AltosMap implements AltosMapTileListener, AltosMapStoreListener { return false; } - public void show(AltosGPS gps, int state) { - + public void show(AltosGPS gps, double time, int state, double gps_height) { /* * If insufficient gps data, nothing to update */ @@ -250,7 +249,7 @@ public class AltosMap implements AltosMapTileListener, AltosMapStoreListener { } if (path != null) { - AltosMapRectangle damage = path.add(gps.lat, gps.lon, state); + AltosMapRectangle damage = path.add(gps, time, state, gps_height); if (damage != null) repaint(damage, AltosMapPath.stroke_width); @@ -262,7 +261,7 @@ public class AltosMap implements AltosMapTileListener, AltosMapStoreListener { } public void show(AltosState state, AltosListenerState listener_state) { - show(state.gps, state.state()); + show(state.gps, state.time, state.state(), state.gps_height()); } public void centre(AltosLatLon lat_lon) { @@ -290,13 +289,19 @@ public class AltosMap implements AltosMapTileListener, AltosMapStoreListener { centre(lat_lon); } - public void add_mark(double lat, double lon, int state) { + public AltosMapMark add_mark(double lat, double lon, int state) { + AltosMapMark mark; synchronized(marks) { - AltosMapMark mark = map_interface.new_mark(lat, lon, state); + mark = map_interface.new_mark(lat, lon, state); if (mark != null) marks.add(mark); } repaint(); + return mark; + } + + public void del_mark(AltosMapMark mark) { + marks.remove(mark); } public void clear_marks() { @@ -490,6 +495,14 @@ public class AltosMap implements AltosMapTileListener, AltosMapStoreListener { drag_stop(x, y); } + public AltosMapPathPoint nearest(int x, int y) { + notice_user_input(); + if (path == null) + return null; + AltosLatLon at = transform.screen_lat_lon(new AltosPointInt(x, y)); + return path.nearest(at); + } + public AltosMap(AltosMapInterface map_interface, int scale) { this.map_interface = map_interface; this.scale = scale; diff --git a/altoslib/AltosMapPath.java b/altoslib/AltosMapPath.java index 7104b2f6..bbb6c994 100644 --- a/altoslib/AltosMapPath.java +++ b/altoslib/AltosMapPath.java @@ -32,19 +32,44 @@ public abstract class AltosMapPath { public abstract void paint(AltosMapTransform t); - public AltosMapRectangle add(double lat, double lon, int state) { - AltosMapPathPoint point = new AltosMapPathPoint(new AltosLatLon (lat, lon), state); + public AltosMapRectangle add(AltosGPS gps, double time, int state, double gps_height) { + AltosMapPathPoint point = new AltosMapPathPoint(gps, time, state, gps_height); AltosMapRectangle rect = null; if (!point.equals(last_point)) { if (last_point != null) - rect = new AltosMapRectangle(last_point.lat_lon, point.lat_lon); + rect = new AltosMapRectangle(last_point.gps.lat_lon(), point.gps.lat_lon()); points.add (point); last_point = point; } return rect; } + private double dist(AltosLatLon lat_lon, AltosMapPathPoint point) { + return (new AltosGreatCircle(lat_lon.lat, + lat_lon.lon, + point.gps.lat, + point.gps.lon)).distance; + } + + public AltosMapPathPoint nearest(AltosLatLon lat_lon) { + AltosMapPathPoint nearest = null; + double nearest_dist = 0; + for (AltosMapPathPoint point : points) { + if (nearest == null) { + nearest = point; + nearest_dist = dist(lat_lon, point); + } else { + double d = dist(lat_lon, point); + if (d < nearest_dist) { + nearest = point; + nearest_dist = d; + } + } + } + return nearest; + } + public void clear () { points = new LinkedList<AltosMapPathPoint>(); } diff --git a/altoslib/AltosMapPathPoint.java b/altoslib/AltosMapPathPoint.java index 9a1edd42..88a8bfcf 100644 --- a/altoslib/AltosMapPathPoint.java +++ b/altoslib/AltosMapPathPoint.java @@ -24,11 +24,13 @@ import java.util.*; import java.util.concurrent.*; public class AltosMapPathPoint { - public AltosLatLon lat_lon; + public AltosGPS gps; + public double time; public int state; + public double gps_height; public int hashCode() { - return lat_lon.hashCode() ^ state; + return Double.valueOf(gps.lat).hashCode() ^ Double.valueOf(gps.lon).hashCode() ^ state; } public boolean equals(Object o) { @@ -40,12 +42,14 @@ public class AltosMapPathPoint { AltosMapPathPoint other = (AltosMapPathPoint) o; - return lat_lon.equals(other.lat_lon) && state == other.state; + return gps.lat == other.gps.lat && gps.lon == other.gps.lon && state == other.state; } - public AltosMapPathPoint(AltosLatLon lat_lon, int state) { - this.lat_lon = lat_lon; + public AltosMapPathPoint(AltosGPS gps, double time, int state, double gps_height) { + this.gps = gps; + this.time = time; this.state = state; + this.gps_height = gps_height; } } diff --git a/altoslib/AltosMapTransform.java b/altoslib/AltosMapTransform.java index 6bc6c6f8..9eac2654 100644 --- a/altoslib/AltosMapTransform.java +++ b/altoslib/AltosMapTransform.java @@ -67,13 +67,13 @@ public class AltosMapTransform { return lat_lon(screen_point(screen)); } - public AltosPointDouble point(AltosLatLon lat_lon) { + public AltosPointDouble point(double lat, double lon) { double x, y; double e; - x = lat_lon.lon * scale_x; + x = lon * scale_x; - e = Math.sin(Math.toRadians(lat_lon.lat)); + e = Math.sin(Math.toRadians(lat)); e = Math.max(e,-(1-1.0E-15)); e = Math.min(e, 1-1.0E-15 ); @@ -82,6 +82,10 @@ public class AltosMapTransform { return new AltosPointDouble(x, y); } + public AltosPointDouble point(AltosLatLon lat_lon) { + return point(lat_lon.lat, lat_lon.lon); + } + public AltosPointDouble screen(AltosPointDouble point) { return new AltosPointDouble(point.x - offset_x, point.y - offset_y); } @@ -102,6 +106,10 @@ public class AltosMapTransform { return screen(point(lat_lon)); } + public AltosPointDouble screen(double lat, double lon) { + return screen(point(lat, lon)); + } + private boolean has_location; public boolean has_location() { diff --git a/altosui/AltosGraphUI.java b/altosui/AltosGraphUI.java index 59b06c18..c1fdc3a3 100644 --- a/altosui/AltosGraphUI.java +++ b/altosui/AltosGraphUI.java @@ -45,6 +45,7 @@ public class AltosGraphUI extends AltosUIFrame implements AltosFontListener, Alt void fill_map(AltosFlightSeries flight_series) { boolean any_gps = false; AltosGPSTimeValue gtv_last = null; + double gps_pad_altitude = flight_series.cal_data().gps_pad_altitude;; if (flight_series.gps_series != null) { for (AltosGPSTimeValue gtv : flight_series.gps_series) { @@ -54,7 +55,9 @@ public class AltosGraphUI extends AltosUIFrame implements AltosFontListener, Alt gps.nsat >= 4) { if (map == null) map = new AltosUIMap(); - map.show(gps, (int) flight_series.value_before(AltosFlightSeries.state_name, gtv.time)); + double gps_height = gps.alt - gps_pad_altitude; + int state = (int) flight_series.value_before(AltosFlightSeries.state_name, gtv.time); + map.show(gps, gtv.time, state, gps_height); this.gps = gps; gtv_last = gtv; has_gps = true; @@ -63,8 +66,9 @@ public class AltosGraphUI extends AltosUIFrame implements AltosFontListener, Alt } if (gtv_last != null) { int state = (int) flight_series.value_after(AltosFlightSeries.state_name, gtv_last.time); + double gps_height = gps.alt - gps_pad_altitude; if (state == AltosLib.ao_flight_landed) - map.show(gtv_last.gps, state); + map.show(gtv_last.gps, gtv_last.time, state,gps_height); } } diff --git a/altosuilib/AltosUIMap.java b/altosuilib/AltosUIMap.java index 8dfdba64..5b981d14 100644 --- a/altosuilib/AltosUIMap.java +++ b/altosuilib/AltosUIMap.java @@ -36,6 +36,7 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM Graphics2D g; Font tile_font; Font line_font; + AltosMapMark nearest_mark; static Point2D.Double point2d(AltosPointDouble pt) { return new Point2D.Double(pt.x, pt.y); @@ -115,7 +116,61 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM map.touch_continue(e.getPoint().x, e.getPoint().y, is_drag_event(e)); } + String pos(double p, String pos, String neg) { + if (p == AltosLib.MISSING) + return ""; + String h = pos; + if (p < 0) { + h = neg; + p = -p; + } + int deg = (int) Math.floor(p); + double min = (p - Math.floor(p)) * 60.0; + return String.format("%s %4d° %9.6f'", h, deg, min); + } + + String height(double h, String label) { + if (h == AltosLib.MISSING) + return ""; + return String.format(" %s%s", + AltosConvert.height.show(6, h), + label); + } + + String speed(double s, String label) { + if (s == AltosLib.MISSING) + return ""; + return String.format(" %s%s", + AltosConvert.speed.show(6, s), + label); + } + public void mouseMoved(MouseEvent e) { + AltosMapPathPoint point = map.nearest(e.getPoint().x, e.getPoint().y); + + if (nearest_mark == null) + nearest_mark = map.add_mark(point.gps.lat, + point.gps.lon, + point.state); + else { + nearest_mark.lat_lon.lat = point.gps.lat; + nearest_mark.lat_lon.lon = point.gps.lon; + nearest_mark.state = point.state; + } + if (point != null) { + nearest_label.setText(String.format("%9.2f sec %s%s%s%s", + point.time, + pos(point.gps.lat, + "N", "S"), + pos(point.gps.lon, + "E", "W"), + height(point.gps_height, ""), + speed(point.gps.ground_speed, "(h)"), + speed(point.gps.climb_rate, "(v)"))); + } else { + nearest_label.setText(""); + } + repaint(); } /* MouseListener methods */ @@ -222,7 +277,7 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM g.setStroke(new BasicStroke(stroke_width, BasicStroke.CAP_ROUND, BasicStroke.JOIN_ROUND)); for (AltosMapPathPoint point : points) { - Point2D.Double cur = point2d(t.screen(point.lat_lon)); + Point2D.Double cur = point2d(t.screen(point.gps.lat, point.gps.lon)); if (prev != null) { Line2D.Double line = new Line2D.Double (prev, cur); Rectangle bounds = line.getBounds(); @@ -387,6 +442,8 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM public void set_font() { tile_font = AltosUILib.value_font; line_font = AltosUILib.status_font; + if (nearest_label != null) + nearest_label.setFont(AltosUILib.value_font); } public void font_size_changed(int font_size) { @@ -400,6 +457,8 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM JLabel zoom_label; + JLabel nearest_label; + public void set_maptype(int type) { /* map.set_maptype(type); @@ -430,8 +489,8 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM map.show(state, listener_state); } - public void show(AltosGPS gps, int state) { - map.show(gps, state); + public void show(AltosGPS gps, double time, int state, double gps_height) { + map.show(gps, time, state, gps_height); } public String getName() { @@ -541,6 +600,20 @@ public class AltosUIMap extends JComponent implements AltosFlightDisplay, AltosM c.weighty = 0; add(zoom_out, c); + + nearest_label = new JLabel("", JLabel.LEFT); + nearest_label.setFont(tile_font); + + c = new GridBagConstraints(); + c.anchor = GridBagConstraints.CENTER; + c.fill = GridBagConstraints.HORIZONTAL; + c.gridx = 0; + c.gridy = 11; + c.weightx = 0; + c.weighty = 0; + c.gridwidth = 1; + c.gridheight = 1; + add(nearest_label, c); /* maptype_combo = new JComboBox<String>(map.maptype_labels); diff --git a/src/drivers/ao_25lc1024.c b/src/drivers/ao_25lc1024.c index c73f54df..2139dadc 100644 --- a/src/drivers/ao_25lc1024.c +++ b/src/drivers/ao_25lc1024.c @@ -169,7 +169,7 @@ ao_storage_device_write(uint32_t pos, void *buf, uint16_t len) ao_ee_flush_internal(); ao_ee_block = block; } - ao_xmemcpy(ao_ee_data + (uint16_t) (pos & 0xff), buf, len); + memcpy(ao_ee_data + (uint16_t) (pos & 0xff), buf, len); ao_ee_block_dirty = 1; } ao_mutex_put(&ao_ee_mutex); return 1; @@ -183,7 +183,7 @@ ao_storage_device_read(uint32_t pos, void *buf, uint16_t len) /* Transfer the data */ ao_mutex_get(&ao_ee_mutex); { ao_ee_fill(block); - ao_xmemcpy(buf, ao_ee_data + (uint16_t) (pos & 0xff), len); + memcpy(buf, ao_ee_data + (uint16_t) (pos & 0xff), len); } ao_mutex_put(&ao_ee_mutex); return 1; } @@ -202,7 +202,7 @@ ao_storage_erase(uint32_t pos) ao_mutex_get(&ao_ee_mutex); { ao_ee_flush_internal(); ao_ee_block = (uint16_t) (pos >> EE_BLOCK_SHIFT); - ao_xmemset(ao_ee_data, 0xff, EE_BLOCK_SIZE); + memset(ao_ee_data, 0xff, EE_BLOCK_SIZE); ao_ee_block_dirty = 1; } ao_mutex_put(&ao_ee_mutex); return 1; diff --git a/src/drivers/ao_aprs.c b/src/drivers/ao_aprs.c index 2f17d044..bc5c29df 100644 --- a/src/drivers/ao_aprs.c +++ b/src/drivers/ao_aprs.c @@ -527,7 +527,7 @@ static int tncComment(uint8_t *buf) int16_t apogee = ao_ignite_decivolt(AO_SENSE_DROGUE(&packet)); #endif #ifdef AO_SENSE_MAIN - int16_t main = ao_ignite_decivolt(AO_SENSE_MAIN(&packet)); + int16_t main_value = ao_ignite_decivolt(AO_SENSE_MAIN(&packet)); #endif return sprintf((char *) buf, @@ -548,8 +548,8 @@ static int tncComment(uint8_t *buf) apogee%10 #endif #ifdef AO_SENSE_MAIN - , main/10, - main%10 + , main_value/10, + main_value%10 #endif , ao_serial_number ); diff --git a/src/drivers/ao_at45db161d.c b/src/drivers/ao_at45db161d.c index 516811b8..e04baaf6 100644 --- a/src/drivers/ao_at45db161d.c +++ b/src/drivers/ao_at45db161d.c @@ -246,7 +246,7 @@ ao_storage_device_write(uint32_t pos, void *buf, uint16_t len) ao_flash_flush_internal(); ao_flash_block = block; } - ao_xmemcpy(ao_flash_data + (uint16_t) (pos & ao_flash_block_mask), + memcpy(ao_flash_data + (uint16_t) (pos & ao_flash_block_mask), buf, len); ao_flash_block_dirty = 1; @@ -262,7 +262,7 @@ ao_storage_device_read(uint32_t pos, void *buf, uint16_t len) /* Transfer the data */ ao_mutex_get(&ao_flash_mutex); { ao_flash_fill(block); - ao_xmemcpy(buf, + memcpy(buf, ao_flash_data + (uint16_t) (pos & ao_flash_block_mask), len); } ao_mutex_put(&ao_flash_mutex); @@ -283,7 +283,7 @@ ao_storage_erase(uint32_t pos) ao_mutex_get(&ao_flash_mutex); { ao_flash_flush_internal(); ao_flash_block = (uint16_t) (pos >> ao_flash_block_shift); - ao_xmemset(ao_flash_data, 0xff, ao_flash_block_size); + memset(ao_flash_data, 0xff, ao_flash_block_size); ao_flash_block_dirty = 1; } ao_mutex_put(&ao_flash_mutex); return 1; diff --git a/src/drivers/ao_bmx160.c b/src/drivers/ao_bmx160.c new file mode 100644 index 00000000..e1af23a1 --- /dev/null +++ b/src/drivers/ao_bmx160.c @@ -0,0 +1,424 @@ +/* + * Copyright © 2019 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_bmx160.h> +#include <ao_exti.h> + +static uint8_t ao_bmx160_configured; + +#define ao_bmx160_spi_get() ao_spi_get(AO_BMX160_SPI_BUS, AO_SPI_SPEED_8MHz) +#define ao_bmx160_spi_put() ao_spi_put(AO_BMX160_SPI_BUS) + +#define ao_bmx160_spi_start() ao_spi_set_cs(AO_BMX160_SPI_CS_PORT, \ + (1 << AO_BMX160_SPI_CS_PIN)) + +#define ao_bmx160_spi_end() ao_spi_clr_cs(AO_BMX160_SPI_CS_PORT, \ + (1 << AO_BMX160_SPI_CS_PIN)) + +static void +_ao_bmx160_reg_write(uint8_t addr, uint8_t value) +{ + uint8_t d[2] = { addr, value }; + ao_bmx160_spi_start(); + ao_spi_send(d, 2, AO_BMX160_SPI_BUS); + ao_bmx160_spi_end(); +} + +static void +_ao_bmx160_read(uint8_t addr, void *data, uint8_t len) +{ + addr |= 0x80; + ao_bmx160_spi_start(); + ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS); + ao_spi_recv(data, len, AO_BMX160_SPI_BUS); + ao_bmx160_spi_end(); +} + +static uint8_t +_ao_bmx160_reg_read(uint8_t addr) +{ + uint8_t value; + addr |= 0x80; + ao_bmx160_spi_start(); + ao_spi_send(&addr, 1, AO_BMX160_SPI_BUS); + ao_spi_recv(&value, 1, AO_BMX160_SPI_BUS); + ao_bmx160_spi_end(); + return value; +} + +static void +_ao_bmx160_cmd(uint8_t cmd) +{ + _ao_bmx160_reg_write(BMX160_CMD, cmd); + ao_delay(AO_MS_TO_TICKS(100)); +} + +static void +_ao_bmx160_mag_setup(void) +{ + _ao_bmx160_reg_write(BMX160_MAG_IF_0, 0x80); +} + +static void +_ao_bmm150_wait_manual(void) +{ + while (_ao_bmx160_reg_read(BMX160_STATUS) & (1 << BMX160_STATUS_MAG_MAN_OP)) + ; +} + +static void +_ao_bmm150_reg_write(uint8_t addr, uint8_t data) +{ + _ao_bmx160_reg_write(BMX160_MAG_IF_3, data); + _ao_bmx160_reg_write(BMX160_MAG_IF_2, addr); + _ao_bmm150_wait_manual(); +} + +#if BMX160_TEST +static uint8_t +_ao_bmm150_reg_read(uint8_t addr) +{ + _ao_bmx160_reg_write(BMX160_MAG_IF_1, addr); + _ao_bmm150_wait_manual(); + return _ao_bmx160_reg_read(BMX160_DATA_0); +} +#endif + +static void +_ao_bmx160_sample(struct ao_bmx160_sample *sample) +{ + _ao_bmx160_read(BMX160_MAG_X_0_7, sample, sizeof (*sample)); +#if __BYTE_ORDER != __LITTLE_ENDIAN + int i = sizeof (*sample) / 2; + uint16_t *d = (uint16_t *) sample; + + /* byte swap */ + while (i--) { + uint16_t t = *d; + *d++ = (t >> 8) | (t << 8); + } +#endif +} + +#define G 981 /* in cm/s² */ + +#if 0 +static int16_t /* cm/s² */ +ao_bmx160_accel(int16_t v) +{ + return (int16_t) ((v * (int32_t) (16.0 * 980.665 + 0.5)) / 32767); +} + +static int16_t /* deg*10/s */ +ao_bmx160_gyro(int16_t v) +{ + return (int16_t) ((v * (int32_t) 20000) / 32767); +} + +static uint8_t +ao_bmx160_accel_check(int16_t normal, int16_t test) +{ + int16_t diff = test - normal; + + if (diff < BMX160_ST_ACCEL(16) / 4) { + return 1; + } + if (diff > BMX160_ST_ACCEL(16) * 4) { + return 1; + } + return 0; +} + +static uint8_t +ao_bmx160_gyro_check(int16_t normal, int16_t test) +{ + int16_t diff = test - normal; + + if (diff < 0) + diff = -diff; + if (diff < BMX160_ST_GYRO(2000) / 4) { + return 1; + } + if (diff > BMX160_ST_GYRO(2000) * 4) { + return 1; + } + return 0; +} +#endif + +static void +_ao_bmx160_wait_alive(void) +{ + uint8_t i; + + /* Wait for the chip to wake up */ + for (i = 0; i < 30; i++) { + ao_delay(AO_MS_TO_TICKS(100)); + if (_ao_bmx160_reg_read(BMX160_CHIPID) == BMX160_CHIPID_BMX160) + break; + } + if (i == 30) + ao_panic(AO_PANIC_SELF_TEST_BMX160); +} + +#define ST_TRIES 10 +#define MAG_TRIES 10 + +static void +_ao_bmx160_setup(void) +{ + if (ao_bmx160_configured) + return; + + /* Make sure the chip is responding */ + _ao_bmx160_wait_alive(); + + /* Reboot */ + _ao_bmx160_cmd(BMX160_CMD_SOFTRESET); + + /* Force SPI mode */ + _ao_bmx160_reg_write(BMX160_NV_CONF, 1 << BMX160_NV_CONF_SPI_EN); + + /* Configure accelerometer: + * + * undersampling disabled + * normal filter + * 200Hz sampling rate + * 16g range + * + * This yields a 3dB cutoff frequency of 80Hz + */ + _ao_bmx160_reg_write(BMX160_ACC_CONF, + (0 << BMX160_ACC_CONF_ACC_US) | + (BMX160_ACC_CONF_ACC_BWP_NORMAL << BMX160_ACC_CONF_ACC_BWP) | + (BMX160_ACC_CONF_ACC_ODR_200 << BMX160_ACC_CONF_ACC_ODR)); + _ao_bmx160_reg_write(BMX160_ACC_RANGE, + BMX160_ACC_RANGE_16G); + + /* Configure gyro: + * + * 200Hz sampling rate + * Normal filter mode + * ±2000°/s + */ + _ao_bmx160_reg_write(BMX160_GYR_CONF, + (BMX160_GYR_CONF_GYR_BWP_NORMAL << BMX160_GYR_CONF_GYR_BWP) | + (BMX160_GYR_CONF_GYR_ODR_200 << BMX160_GYR_CONF_GYR_ODR)); + _ao_bmx160_reg_write(BMX160_GYR_RANGE, + BMX160_GYR_RANGE_2000); + + + /* Configure magnetometer: + * + * 30Hz sampling rate + * power on + * axes enabled + */ + _ao_bmx160_cmd(BMX160_CMD_MAG_IF_SET_PMU_MODE(BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL)); + + /* Enter setup mode */ + _ao_bmx160_mag_setup(); + + /* Place in suspend mode to reboot the chip */ + _ao_bmm150_reg_write(BMM150_POWER_MODE, + (0 << BMM150_POWER_MODE_POWER_CONTROL)); + + /* Power on */ + _ao_bmm150_reg_write(BMM150_POWER_MODE, + (1 << BMM150_POWER_MODE_POWER_CONTROL)); + + /* Set data rate and place in sleep mode */ + _ao_bmm150_reg_write(BMM150_CONTROL, + (BMM150_CONTROL_DATA_RATE_30 << BMM150_CONTROL_DATA_RATE) | + (BMM150_CONTROL_OP_MODE_SLEEP << BMM150_CONTROL_OP_MODE)); + + /* enable all axes (should already be enabled) */ + _ao_bmm150_reg_write(BMM150_INT_CONF, + (0 << BMM150_INT_CONF_X_DISABLE) | + (0 << BMM150_INT_CONF_Y_DISABLE) | + (0 << BMM150_INT_CONF_Z_DISABLE)); + + /* Set repetition values (?) */ + _ao_bmm150_reg_write(BMM150_REPXY, BMM150_REPXY_VALUE(9)); + _ao_bmm150_reg_write(BMM150_REPZ, BMM150_REPZ_VALUE(15)); + + /* To get data out of the magnetometer, set the control op mode to 'forced', then read + * from the data registers + */ + _ao_bmx160_reg_write(BMX160_MAG_IF_3, (BMM150_CONTROL_OP_MODE_FORCED << BMM150_CONTROL_OP_MODE)); + _ao_bmx160_reg_write(BMX160_MAG_IF_2, BMM150_CONTROL); + _ao_bmx160_reg_write(BMX160_MAG_IF_1, BMM150_DATA_X_0_4); + + /* Set data rate to 200Hz */ + _ao_bmx160_reg_write(BMX160_MAG_CONF, + (BMX160_MAG_CONF_MAG_ODR_200 << BMX160_MAG_CONF_MAG_ODR)); + + /* Put magnetometer interface back into 'normal mode' + */ + _ao_bmx160_reg_write(BMX160_MAG_IF_0, + (0 << BMX160_MAG_IF_0_MAG_MANUAL_EN) | + (0 << BMX160_MAG_IF_0_MAG_OFFSET) | + (0 << BMX160_MAG_IF_0_MAG_RD_BURST)); + + /* Enable acc and gyr + */ + + _ao_bmx160_cmd(BMX160_CMD_ACC_SET_PMU_MODE(BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL)); + _ao_bmx160_cmd(BMX160_CMD_GYR_SET_PMU_MODE(BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL)); + ao_bmx160_configured = 1; +} + +struct ao_bmx160_sample ao_bmx160_current; + +static void +ao_bmx160(void) +{ + struct ao_bmx160_sample sample; + + /* ao_bmx160_init already grabbed the SPI bus and mutex */ + _ao_bmx160_setup(); + ao_bmx160_spi_put(); + for (;;) + { + ao_bmx160_spi_get(); + _ao_bmx160_sample(&sample); + ao_bmx160_spi_put(); + ao_arch_block_interrupts(); + ao_bmx160_current = sample; + AO_DATA_PRESENT(AO_DATA_BMX160); + AO_DATA_WAIT(); + ao_arch_release_interrupts(); + } +} + +static struct ao_task ao_bmx160_task; + +static void +ao_bmx160_show(void) +{ + printf ("Accel: %7d %7d %7d Gyro: %7d %7d %7d Mag: %7d %7d %7d\n", + ao_bmx160_current.acc_x, + ao_bmx160_current.acc_y, + ao_bmx160_current.acc_z, + ao_bmx160_current.gyr_x, + ao_bmx160_current.gyr_y, + ao_bmx160_current.gyr_z, + ao_bmx160_current.mag_x, + ao_bmx160_current.mag_y, + ao_bmx160_current.mag_z); +} + +#if BMX160_TEST + +static void +ao_bmx160_read(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_bmx160_spi_get(); + val = _ao_bmx160_reg_read(addr); + ao_bmx160_spi_put(); + printf("Addr %02x val %02x\n", addr, val); +} + +static void +ao_bmx160_write(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + val = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + printf("Addr %02x val %02x\n", addr, val); + ao_bmx160_spi_get(); + _ao_bmx160_reg_write(addr, val); + ao_bmx160_spi_put(); +} + +static void +ao_bmm150_read(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + ao_bmx160_spi_get(); + val = _ao_bmm150_reg_read(addr); + ao_bmx160_spi_put(); + printf("Addr %02x val %02x\n", addr, val); +} + +static void +ao_bmm150_write(void) +{ + uint8_t addr; + uint8_t val; + + addr = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + val = ao_cmd_hex(); + if (ao_cmd_status != ao_cmd_success) + return; + printf("Addr %02x val %02x\n", addr, val); + ao_bmx160_spi_get(); + _ao_bmm150_reg_write(addr, val); + ao_bmx160_spi_put(); +} + +#endif /* BMX160_TEST */ + +static const struct ao_cmds ao_bmx160_cmds[] = { + { ao_bmx160_show, "I\0Show BMX160 status" }, +#if BMX160_TEST + { ao_bmx160_read, "R <addr>\0Read BMX160 register" }, + { ao_bmx160_write, "W <addr> <val>\0Write BMX160 register" }, + { ao_bmm150_read, "M <addr>\0Read BMM150 register" }, + { ao_bmm150_write, "N <addr> <val>\0Write BMM150 register" }, +#endif + { 0, NULL } +}; + +void +ao_bmx160_init(void) +{ + ao_add_task(&ao_bmx160_task, ao_bmx160, "bmx160"); + + ao_spi_init_cs(AO_BMX160_SPI_CS_PORT, (1 << AO_BMX160_SPI_CS_PIN)); + + /* Pretend to be the bmx160 task. Grab the SPI bus right away and + * hold it for the task so that nothing else uses the SPI bus before + * we get the I2C mode disabled in the chip + */ + + ao_cur_task = &ao_bmx160_task; + ao_bmx160_spi_get(); + ao_cur_task = NULL; + ao_cmd_register(&ao_bmx160_cmds[0]); +} diff --git a/src/drivers/ao_bmx160.h b/src/drivers/ao_bmx160.h new file mode 100644 index 00000000..2b7e7bd7 --- /dev/null +++ b/src/drivers/ao_bmx160.h @@ -0,0 +1,269 @@ +/* + * Copyright © 2019 Keith Packard <keithp@keithp.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_BMX160_H_ +#define _AO_BMX160_H_ + +#include <math.h> + +struct ao_bmx160_sample { + int16_t mag_x; + int16_t mag_y; + int16_t mag_z; + int16_t rhall; + int16_t gyr_x; + int16_t gyr_y; + int16_t gyr_z; + int16_t acc_x; + int16_t acc_y; + int16_t acc_z; +}; + +extern struct ao_bmx160_sample ao_bmx160_current; + +struct ao_bmx160_offset { + int8_t off_acc_x; + int8_t off_acc_y; + int8_t off_acc_z; + int8_t off_gyr_x; + int8_t off_gyr_y; + int8_t off_gyr_z; + uint8_t offset_6; +}; + +void +ao_bmx160_init(void); + +#define BMX160_CHIPID 0x00 +#define BMX160_CHIPID_BMX160 0xd8 +#define BMX160_ERR_REG 0x02 +#define BMX160_PMU_STATUS 0x03 +#define BMX160_PMU_STATUS_MAG_IF_PMU_STATUS 0 +#define BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_SUSPEND 0 +#define BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_NORMAL 1 +#define BMX160_PMU_STATUS_MAG_IF_PMU_STATUS_LOW_POWER 2 +#define BMX160_PMU_STATUS_GYR_PMU_STATUS 2 +#define BMX160_PMU_STATUS_GYR_PMU_STATUS_SUSPEND 0 +#define BMX160_PMU_STATUS_GYR_PMU_STATUS_NORMAL 1 +#define BMX160_PMU_STATUS_GYR_PMU_STATUS_FAST_START_UP 3 +#define BMX160_PMU_STATUS_ACC_PMU_STATUS 4 +#define BMX160_PMU_STATUS_ACC_PMU_STATUS_SUSPEND 0 +#define BMX160_PMU_STATUS_ACC_PMU_STATUS_NORMAL 1 +#define BMX160_PMU_STATUS_ACC_PMU_STATUS_LOW_POWER 2 +#define BMX160_DATA_0 0x04 +#define BMX160_MAG_X_0_7 0x04 +#define BMX160_MAG_X_8_15 0x05 +#define BMX160_MAG_Y_0_7 0x06 +#define BMX160_MAG_Y_8_15 0x07 +#define BMX160_MAG_Z_0_7 0x08 +#define BMX160_MAG_Z_8_15 0x09 +#define BMX160_RHALL_0_7 0x0A +#define BMX160_RHALL_8_15 0x0B +#define BMX160_GYRO_X_0_7 0x0C +#define BMX160_GYRO_X_8_15 0x0D +#define BMX160_GYRO_Y_0_7 0x0E +#define BMX160_GYRO_Y_8_15 0x0F +#define BMX160_GYRO_Z_0_7 0x10 +#define BMX160_GYRO_Z_8_15 0x11 +#define BMX160_ACCEL_X_0_7 0x12 +#define BMX160_ACCEL_X_8_15 0x13 +#define BMX160_ACCEL_Y_0_7 0x14 +#define BMX160_ACCEL_Y_8_15 0x15 +#define BMX160_ACCEL_Z_0_7 0x16 +#define BMX160_ACCEL_Z_8_15 0x17 +#define BMX160_SENSORTIME_0_7 0x18 +#define BMX160_SENSORTIME_8_15 0x19 +#define BMX160_SENSORTIME_16_23 0x1A +#define BMX160_STATUS 0x1B +#define BMX160_STATUS_GYR_SELF_TEST_OK 1 +#define BMX160_STATUS_MAG_MAN_OP 2 +#define BMX160_STATUS_FOC_RDY 3 +#define BMX160_STATUS_NVM_RDY 4 +#define BMX160_STATUS_DRDY_MAG 5 +#define BMX160_STATUS_DRDY_GYR 6 +#define BMX160_STATUS_DRDY_ACC 7 +#define BMX160_INT_STATUS_0 0x1C-0x1F +#define BMX160_INT_STATUS_1 0x1D +#define BMX160_INT_STATUS_2 0x1E +#define BMX160_INT_STATUS_3 0x1F +#define BMX160_TEMPERATURE_0_7 0x20 +#define BMX160_TEMPERATURE_8_15 0x21 +#define BMX160_FIFO_LENGTH_0_7 0x22 +#define BMX160_FIFO_LENGTH_8_15 0x23 +#define BMX160_FIFO_DATA 0x24 +#define BMX160_ACC_CONF 0x40 +#define BMX160_ACC_CONF_ACC_ODR 0 +#define BMX160_ACC_CONF_ACC_ODR_25_32 0x1 +#define BMX160_ACC_CONF_ACC_ODR_25_16 0x2 +#define BMX160_ACC_CONF_ACC_ODR_25_8 0x3 +#define BMX160_ACC_CONF_ACC_ODR_25_4 0x4 +#define BMX160_ACC_CONF_ACC_ODR_25_2 0x5 +#define BMX160_ACC_CONF_ACC_ODR_25 0x6 +#define BMX160_ACC_CONF_ACC_ODR_50 0x7 +#define BMX160_ACC_CONF_ACC_ODR_100 0x8 +#define BMX160_ACC_CONF_ACC_ODR_200 0x9 +#define BMX160_ACC_CONF_ACC_ODR_400 0xa +#define BMX160_ACC_CONF_ACC_ODR_800 0xb +#define BMX160_ACC_CONF_ACC_ODR_1600 0xc +#define BMX160_ACC_CONF_ACC_BWP 4 +#define BMX160_ACC_CONF_ACC_BWP_NORMAL 0x2 +#define BMX160_ACC_CONF_ACC_BWP_OSR2 0x1 +#define BMX160_ACC_CONF_ACC_BWP_OSR4 0x0 +#define BMX160_ACC_CONF_ACC_US 7 +#define BMX160_ACC_RANGE 0x41 +#define BMX160_ACC_RANGE_2G 0x3 +#define BMX160_ACC_RANGE_4G 0x5 +#define BMX160_ACC_RANGE_8G 0x8 +#define BMX160_ACC_RANGE_16G 0xc +#define BMX160_ACC_RANGE_ +#define BMX160_ACC_RANGE_ +#define BMX160_GYR_CONF 0x42 +#define BMX160_GYR_CONF_GYR_ODR 0 +#define BMX160_GYR_CONF_GYR_ODR_25 0x6 +#define BMX160_GYR_CONF_GYR_ODR_50 0x7 +#define BMX160_GYR_CONF_GYR_ODR_100 0x8 +#define BMX160_GYR_CONF_GYR_ODR_200 0x9 +#define BMX160_GYR_CONF_GYR_ODR_400 0xa +#define BMX160_GYR_CONF_GYR_ODR_800 0xb +#define BMX160_GYR_CONF_GYR_ODR_1600 0xc +#define BMX160_GYR_CONF_GYR_ODR_3200 0xd +#define BMX160_GYR_CONF_GYR_BWP 4 +#define BMX160_GYR_CONF_GYR_BWP_NORMAL 0x2 +#define BMX160_GYR_CONF_GYR_BWP_OSR2 0x1 +#define BMX160_GYR_CONF_GYR_BWP_OSR4 0x0 +#define BMX160_GYR_RANGE 0x43 +#define BMX160_GYR_RANGE_2000 0x0 +#define BMX160_GYR_RANGE_1000 0x1 +#define BMX160_GYR_RANGE_500 0x2 +#define BMX160_GYR_RANGE_250 0x3 +#define BMX160_GYR_RANGE_125 0x4 +#define BMX160_MAG_CONF 0x44 +#define BMX160_MAG_CONF_MAG_ODR 0 +#define BMX160_MAG_CONF_MAG_ODR_25_32 0x1 +#define BMX160_MAG_CONF_MAG_ODR_25_16 0x2 +#define BMX160_MAG_CONF_MAG_ODR_25_8 0x3 +#define BMX160_MAG_CONF_MAG_ODR_25_4 0x4 +#define BMX160_MAG_CONF_MAG_ODR_25_2 0x5 +#define BMX160_MAG_CONF_MAG_ODR_25 0x6 +#define BMX160_MAG_CONF_MAG_ODR_50 0x7 +#define BMX160_MAG_CONF_MAG_ODR_100 0x8 +#define BMX160_MAG_CONF_MAG_ODR_200 0x9 +#define BMX160_MAG_CONF_MAG_ODR_400 0xa +#define BMX160_MAG_CONF_MAG_ODR_800 0xb +#define BMX160_FIFO_DOWNS 0x45 +#define BMX160_FIFO_CONFIG_0 0x46 +#define BMX160_FIFO_CONFIG_1 0x47 +#define BMX160_MAG_IF_0 0x4C +#define BMX160_MAG_IF_0_MAG_RD_BURST 0 +#define BMX160_MAG_IF_0_MAG_OFFSET 2 +#define BMX160_MAG_IF_0_MAG_MANUAL_EN 7 +#define BMX160_MAG_IF_1 0x4D +#define BMX160_MAG_IF_2 0x4E +#define BMX160_MAG_IF_3 0x4F +#define BMX160_INT_EN 0x50-0x52 +#define BMX160_INT_OUT_CTRL 0x53 +#define BMX160_INT_LATCH 0x54 +#define BMX160_INT_MAP 0x55-0x57 +#define BMX160_INT_DATA 0x58-0x59 +#define BMX160_INT_LOWHIGH 0x5A-0x5E +#define BMX160_INT_MOTION 0x5F-0x62 +#define BMX160_INT_TAP 0x63-0x64 +#define BMX160_INT_ORIENT 0x65-0x66 +#define BMX160_INT_FLAT 0x67-0x68 +#define BMX160_FOC_CONF 0x69 +#define BMX160_CONF 0x6A +#define BMX160_IF_CONF 0x6B +#define BMX160_PMU_TRIGGER 0x6C +#define BMX160_SELF_TEST 0x6D +#define BMX160_NV_CONF 0x70 +#define BMX160_NV_CONF_SPI_EN 0 +#define BMX160_NV_CONF_I2C_WDT_SEL 1 +#define BMX160_NV_CONF_I2C_WDT_EN 2 +#define BMX160_OFFSET 0x71-0x77 +#define BMX160_STEP_CNT 0x78-0x79 +#define BMX160_STEP_CONF 0x7A-0x7B +#define BMX160_CMD 0x7E +#define BMX160_CMD_START_FOC 0x03 +#define BMX160_CMD_ACC_SET_PMU_MODE(n) (0x10 | (n)) +#define BMX160_CMD_GYR_SET_PMU_MODE(n) (0x14 | (n)) +#define BMX160_CMD_MAG_IF_SET_PMU_MODE(n) (0x18 | (n)) +#define BMX160_CMD_PROG_NVM 0xa0 +#define BMX160_CMD_FIFO_FLUSH 0xb0 +#define BMX160_CMD_INT_RESET 0xb1 +#define BMX160_CMD_SOFTRESET 0xb6 +#define BMX160_CMD_STEP_CNT_CLR 0xb2 + +#define BMM150_CHIP_ID 0x40 +#define BMM150_DATA_X_0_4 0x42 +#define BMM150_DATA_X_5_12 0x43 +#define BMM150_DATA_Y_0_4 0x44 +#define BMM150_DATA_Y_5_12 0x45 +#define BMM150_DATA_Z_0_6 0x46 +#define BMM150_DATA_Z_7_14 0x47 +#define BMM150_RHALL_0_5 0x48 +#define BMM150_RHALL_6_13 0x49 +#define BMM150_INT_STATUS 0x4a +#define BMM150_POWER_MODE 0x4b +#define BMM150_POWER_MODE_SOFT_RESET_HI 7 +#define BMM150_POWER_MODE_SPI3EN 2 +#define BMM150_POWER_MODE_SOFT_RESET_LO 1 +#define BMM150_POWER_MODE_POWER_CONTROL 0 +#define BMM150_CONTROL 0x4c +#define BMM150_CONTROL_ADV_ST_1 7 +#define BMM150_CONTROL_ADV_ST_0 6 +#define BMM150_CONTROL_DATA_RATE 3 +#define BMM150_CONTROL_DATA_RATE_10 0 +#define BMM150_CONTROL_DATA_RATE_2 1 +#define BMM150_CONTROL_DATA_RATE_6 2 +#define BMM150_CONTROL_DATA_RATE_8 3 +#define BMM150_CONTROL_DATA_RATE_15 4 +#define BMM150_CONTROL_DATA_RATE_20 5 +#define BMM150_CONTROL_DATA_RATE_25 6 +#define BMM150_CONTROL_DATA_RATE_30 7 +#define BMM150_CONTROL_OP_MODE 1 +#define BMM150_CONTROL_OP_MODE_NORMAL 0 +#define BMM150_CONTROL_OP_MODE_FORCED 1 +#define BMM150_CONTROL_OP_MODE_SLEEP 3 +#define BMM150_CONTROL_SELF_TEST 0 +#define BMM150_INT_EN 0x4d +#define BMM150_INT_CONF 0x4e +#define BMM150_INT_CONF_X_DISABLE 3 +#define BMM150_INT_CONF_Y_DISABLE 4 +#define BMM150_INT_CONF_Z_DISABLE 5 +#define BMM150_LOW_THRESHOLD 0x4f +#define BMM150_HIGH_THRESHOLD 0x50 +#define BMM150_REPXY 0x51 +#define BMM150_REPXY_VALUE(n) (((n)-1) >> 1) +#define BMM150_REPZ 0x52 +#define BMM150_REPZ_VALUE(n) ((n) -1) + +#define BMX160_GYRO_FULLSCALE ((float) 2000 * M_PI/180.0) + +static inline float +ao_bmx160_gyro(float sensor) { + return sensor * ((float) (BMX160_GYRO_FULLSCALE / 32767.0)); +} + +#define BMX160_ACCEL_FULLSCALE 16 + +static inline float +ao_bmx160_accel(int16_t sensor) { + return (float) sensor * ((float) (BMX160_ACCEL_FULLSCALE * GRAVITY / 32767.0)); +} + +#endif /* _BMX160_H_ */ diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c index f767118a..3b4a62ec 100644 --- a/src/drivers/ao_gps_skytraq.c +++ b/src/drivers/ao_gps_skytraq.c @@ -298,7 +298,7 @@ ao_nmea_gga(void) ao_mutex_get(&ao_gps_mutex); ao_gps_new |= AO_GPS_NEW_DATA; ao_gps_tick = ao_gps_next_tick; - ao_xmemcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); + memcpy(&ao_gps_data, &ao_gps_next, sizeof (ao_gps_data)); ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_new); } @@ -357,7 +357,7 @@ ao_nmea_gsv(void) else if (done) { ao_mutex_get(&ao_gps_mutex); ao_gps_new |= AO_GPS_NEW_TRACKING; - ao_xmemcpy(&ao_gps_tracking_data, &ao_gps_tracking_next, sizeof(ao_gps_tracking_data)); + memcpy(&ao_gps_tracking_data, &ao_gps_tracking_next, sizeof(ao_gps_tracking_data)); ao_mutex_put(&ao_gps_mutex); ao_wakeup(&ao_gps_new); } diff --git a/src/drivers/ao_packet.c b/src/drivers/ao_packet.c index b5a0a4b7..b9331c4d 100644 --- a/src/drivers/ao_packet.c +++ b/src/drivers/ao_packet.c @@ -42,7 +42,7 @@ ao_packet_send(void) #endif /* If any tx data is pending then copy it into the tx packet */ if (ao_packet_tx_used && ao_tx_packet.len == 0) { - ao_xmemcpy(&ao_tx_packet.d, tx_data, ao_packet_tx_used); + memcpy(&ao_tx_packet.d, tx_data, ao_packet_tx_used); ao_tx_packet.len = ao_packet_tx_used; ao_tx_packet.seq++; ao_packet_tx_used = 0; @@ -88,10 +88,10 @@ ao_packet_recv(uint16_t timeout) /* Accept packets with matching call signs, or any packet if * our callsign hasn't been configured */ - if (ao_xmemcmp(ao_rx_packet.packet.callsign, + if (memcmp(ao_rx_packet.packet.callsign, ao_config.callsign, AO_MAX_CALLSIGN) != 0 && - ao_xmemcmp(ao_config.callsign, "N0CALL", 7) != 0) + memcmp(ao_config.callsign, "N0CALL", 7) != 0) return 0; /* SYN packets carry no data */ @@ -111,7 +111,7 @@ ao_packet_recv(uint16_t timeout) /* Copy data to the receive data buffer and set up the * offsets */ - ao_xmemcpy(rx_data, ao_rx_packet.packet.d, ao_rx_packet.packet.len); + memcpy(rx_data, ao_rx_packet.packet.d, ao_rx_packet.packet.len); ao_packet_rx_used = 0; ao_packet_rx_len = ao_rx_packet.packet.len; diff --git a/src/drivers/ao_packet_master.c b/src/drivers/ao_packet_master.c index e67dd27f..a44b34cf 100644 --- a/src/drivers/ao_packet_master.c +++ b/src/drivers/ao_packet_master.c @@ -93,7 +93,7 @@ ao_packet_master(void) ao_packet_master_delay = AO_PACKET_MASTER_DELAY_SHORT; while (ao_packet_enable) { uint8_t r; - ao_xmemcpy(ao_tx_packet.callsign, ao_config.callsign, AO_MAX_CALLSIGN); + memcpy(ao_tx_packet.callsign, ao_config.callsign, AO_MAX_CALLSIGN); ao_packet_send(); if (ao_tx_packet.len) ao_packet_master_busy(); diff --git a/src/drivers/ao_packet_slave.c b/src/drivers/ao_packet_slave.c index a5834799..314ce14a 100644 --- a/src/drivers/ao_packet_slave.c +++ b/src/drivers/ao_packet_slave.c @@ -26,7 +26,7 @@ ao_packet_slave(void) ao_packet_restart = 1; while (ao_packet_enable) { if (ao_packet_recv(0)) { - ao_xmemcpy(&ao_tx_packet.callsign, &ao_rx_packet.packet.callsign, AO_MAX_CALLSIGN); + memcpy(&ao_tx_packet.callsign, &ao_rx_packet.packet.callsign, AO_MAX_CALLSIGN); #if HAS_FLIGHT ao_flight_force_idle = true; #endif diff --git a/src/drivers/ao_radio_master.c b/src/drivers/ao_radio_master.c index 8b62a4de..b229bd6f 100644 --- a/src/drivers/ao_radio_master.c +++ b/src/drivers/ao_radio_master.c @@ -134,7 +134,7 @@ ao_radio_get_data(void *d, uint8_t size) AO_RADIO_SPI_REPLY_HEADER_LEN + size, AO_RADIO_SPI_BUS); ao_radio_master_stop(); - ao_xmemcpy(d, ao_radio_spi_reply.payload, size); + memcpy(d, ao_radio_spi_reply.payload, size); PRINTD ("fetched %d\n", size); } @@ -150,7 +150,7 @@ void ao_radio_send(const void *d, uint8_t size) { ao_radio_get(AO_RADIO_SPI_SEND, size); - ao_xmemcpy(&ao_radio_spi_request.payload, d, size); + memcpy(&ao_radio_spi_request.payload, d, size); ao_radio_master_send(); ao_radio_put(); } @@ -190,7 +190,7 @@ ao_radio_cmac_set_key(void) */ PRINTD ("set key\n"); ao_radio_get(AO_RADIO_SPI_CMAC_KEY, AO_AES_LEN); - ao_xmemcpy(&ao_radio_spi_request.payload, &ao_config.aes_key, AO_AES_LEN); + memcpy(&ao_radio_spi_request.payload, &ao_config.aes_key, AO_AES_LEN); ao_radio_master_send(); ao_radio_put(); PRINTD ("key set\n"); @@ -212,7 +212,7 @@ ao_radio_cmac_send(void *packet, uint8_t len) PRINTD ("sending packet\n"); ao_radio_get(AO_RADIO_SPI_CMAC_SEND, len); - ao_xmemcpy(&ao_radio_spi_request.payload, packet, len); + memcpy(&ao_radio_spi_request.payload, packet, len); ao_radio_master_send(); ao_radio_put(); PRINTD ("packet sent\n"); diff --git a/src/drivers/ao_radio_slave.c b/src/drivers/ao_radio_slave.c index 7859d6b6..e0470934 100644 --- a/src/drivers/ao_radio_slave.c +++ b/src/drivers/ao_radio_slave.c @@ -107,7 +107,7 @@ ao_radio_slave_spi(void) break; case AO_RADIO_SPI_CMAC_KEY: - ao_xmemcpy(&ao_config.aes_key, ao_radio_spi_request.payload, AO_AES_LEN); + memcpy(&ao_config.aes_key, ao_radio_spi_request.payload, AO_AES_LEN); break; case AO_RADIO_SPI_TEST_ON: diff --git a/src/easymega-v3.0/.gitignore b/src/easymega-v3.0/.gitignore new file mode 100644 index 00000000..410943d5 --- /dev/null +++ b/src/easymega-v3.0/.gitignore @@ -0,0 +1,2 @@ +ao_product.h +easymega-*.elf diff --git a/src/easymega-v3.0/Makefile b/src/easymega-v3.0/Makefile new file mode 100644 index 00000000..51c51755 --- /dev/null +++ b/src/easymega-v3.0/Makefile @@ -0,0 +1,117 @@ +# +# AltOS build +# +# + +include ../stm/Makefile.defs + +INC = \ + ao.h \ + ao_arch.h \ + ao_arch_funcs.h \ + ao_boot.h \ + ao_companion.h \ + ao_data.h \ + ao_sample.h \ + ao_pins.h \ + altitude-pa.h \ + ao_kalman.h \ + ao_product.h \ + ao_ms5607.h \ + ao_bmx160.h \ + ao_adxl375.h \ + ao_profile.h \ + ao_task.h \ + ao_whiten.h \ + ao_sample_profile.h \ + ao_quaternion.h \ + ao_mpu.h \ + stm32l.h \ + Makefile + +# +# Common AltOS sources +# + +#PROFILE=ao_profile.c +#PROFILE_DEF=-DAO_PROFILE=1 + +#SAMPLE_PROFILE=ao_sample_profile.c \ +# ao_sample_profile_timer.c +#SAMPLE_PROFILE_DEF=-DHAS_SAMPLE_PROFILE=1 + +#STACK_GUARD=ao_mpu_stm.c +#STACK_GUARD_DEF=-DHAS_STACK_GUARD=1 + +ALTOS_SRC = \ + ao_boot_chain.c \ + ao_interrupt.c \ + ao_product.c \ + ao_romconfig.c \ + ao_cmd.c \ + ao_config.c \ + ao_task.c \ + ao_led_stm.c \ + ao_stdio.c \ + ao_panic.c \ + ao_timer.c \ + ao_mutex.c \ + ao_ignite.c \ + ao_freq.c \ + ao_dma_stm.c \ + ao_spi_stm.c \ + ao_data.c \ + ao_ms5607.c \ + ao_bmx160.c \ + ao_adxl375.c \ + ao_adc_stm.c \ + ao_beep_stm.c \ + ao_eeprom_stm.c \ + ao_storage.c \ + ao_m25.c \ + ao_usb_stm.c \ + ao_exti_stm.c \ + ao_report.c \ + ao_i2c_stm.c \ + ao_convert_pa.c \ + ao_convert_volt.c \ + ao_log.c \ + ao_log_mega.c \ + ao_sample.c \ + ao_kalman.c \ + ao_flight.c \ + ao_companion.c \ + ao_pyro.c \ + $(PROFILE) \ + $(SAMPLE_PROFILE) \ + $(STACK_GUARD) + +PRODUCT=EasyMega-v3.0 +PRODUCT_DEF=-DEASYMEGA +IDPRODUCT=0x0028 + +CFLAGS = $(PRODUCT_DEF) $(STM_CFLAGS) $(PROFILE_DEF) $(SAMPLE_PROFILE_DEF) $(STACK_GUARD_DEF) + +PROGNAME=easymega-v3.0 +PROG=$(PROGNAME)-$(VERSION).elf +HEX=$(PROGNAME)-$(VERSION).ihx + +SRC=$(ALTOS_SRC) ao_easymega.c +OBJ=$(SRC:.c=.o) + +all: $(PROG) $(HEX) + +$(PROG): Makefile $(OBJ) altos.ld + $(call quiet,CC) $(LDFLAGS) -o $(PROG) $(OBJ) $(LIBS) + +$(OBJ): $(INC) + +distclean: clean + +clean: + rm -f *.o $(PROGNAME)-*.elf $(PROGNAME)-*.ihx + rm -f ao_product.h + +install: + +uninstall: diff --git a/src/easymega-v3.0/ao_easymega.c b/src/easymega-v3.0/ao_easymega.c new file mode 100644 index 00000000..70a6e409 --- /dev/null +++ b/src/easymega-v3.0/ao_easymega.c @@ -0,0 +1,86 @@ +/* + * Copyright © 2014 Bdale Garbee <bdale@gag.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#include <ao.h> +#include <ao_ms5607.h> +#include <ao_bmx160.h> +#include <ao_adxl375.h> +#include <ao_log.h> +#include <ao_exti.h> +#include <ao_companion.h> +#include <ao_profile.h> +#include <ao_eeprom.h> +#if HAS_SAMPLE_PROFILE +#include <ao_sample_profile.h> +#endif +#include <ao_pyro.h> +#if HAS_STACK_GUARD +#include <ao_mpu.h> +#endif + +int +main(void) +{ + ao_clock_init(); + +#if HAS_STACK_GUARD + ao_mpu_init(); +#endif + + ao_task_init(); + ao_led_init(); + ao_led_on(LEDS_AVAILABLE); + ao_timer_init(); + + ao_i2c_init(); + ao_spi_init(); + ao_dma_init(); + ao_exti_init(); + + ao_adc_init(); + ao_beep_init(); + ao_cmd_init(); + + ao_ms5607_init(); + ao_bmx160_init(); + ao_adxl375_init(); + + ao_eeprom_init(); + ao_storage_init(); + + ao_flight_init(); + ao_log_init(); + ao_report_init(); + + ao_usb_init(); + ao_igniter_init(); + ao_companion_init(); + ao_pyro_init(); + + ao_config_init(); +#if AO_PROFILE + ao_profile_init(); +#endif +#if HAS_SAMPLE_PROFILE + ao_sample_profile_init(); +#endif + + ao_led_off(LEDS_AVAILABLE); + ao_start_scheduler(); + return 0; +} diff --git a/src/easymega-v3.0/ao_pins.h b/src/easymega-v3.0/ao_pins.h new file mode 100644 index 00000000..fa810fda --- /dev/null +++ b/src/easymega-v3.0/ao_pins.h @@ -0,0 +1,366 @@ +/* + * Copyright © 2014 Bdale Garbee <bdale@gag.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +#define HAS_TASK_QUEUE 1 + +/* 16MHz High speed external crystal */ +#define AO_HSE 16000000 + +/* PLLVCO = 96MHz (so that USB will work) */ +#define AO_PLLMUL 6 +#define AO_RCC_CFGR_PLLMUL (STM_RCC_CFGR_PLLMUL_6) + +/* SYSCLK = 32MHz (no need to go faster than CPU) */ +#define AO_PLLDIV 3 +#define AO_RCC_CFGR_PLLDIV (STM_RCC_CFGR_PLLDIV_3) + +/* HCLK = 32MHz (CPU clock) */ +#define AO_AHB_PRESCALER 1 +#define AO_RCC_CFGR_HPRE_DIV STM_RCC_CFGR_HPRE_DIV_1 + +/* Run APB1 at 16MHz (HCLK/2) */ +#define AO_APB1_PRESCALER 2 +#define AO_RCC_CFGR_PPRE1_DIV STM_RCC_CFGR_PPRE2_DIV_2 + +/* Run APB2 at 16MHz (HCLK/2) */ +#define AO_APB2_PRESCALER 2 +#define AO_RCC_CFGR_PPRE2_DIV STM_RCC_CFGR_PPRE2_DIV_2 + +#define HAS_SERIAL_1 0 +#define USE_SERIAL_1_STDIN 0 +#define SERIAL_1_PB6_PB7 0 +#define SERIAL_1_PA9_PA10 0 + +#define HAS_SERIAL_2 0 +#define USE_SERIAL_2_STDIN 0 +#define SERIAL_2_PA2_PA3 0 +#define SERIAL_2_PD5_PD6 0 + +#define HAS_SERIAL_3 0 +#define USE_SERIAL_3_STDIN 0 +#define SERIAL_3_PB10_PB11 0 +#define SERIAL_3_PC10_PC11 0 +#define SERIAL_3_PD8_PD9 0 + +#define ao_gps_getchar ao_serial1_getchar +#define ao_gps_putchar ao_serial1_putchar +#define ao_gps_set_speed ao_serial1_set_speed +#define ao_gps_fifo (ao_stm_usart1.rx_fifo) + +#define AO_CONFIG_DEFAULT_FLIGHT_LOG_MAX (1024 * 1024) +#define AO_CONFIG_MAX_SIZE 1024 +#define LOG_ERASE_MARK 0x55 +#define LOG_MAX_ERASE 128 +#define AO_LOG_FORMAT AO_LOG_FORMAT_EASYMEGA_2 + +#define HAS_EEPROM 1 +#define USE_INTERNAL_FLASH 0 +#define USE_EEPROM_CONFIG 1 +#define USE_STORAGE_CONFIG 0 +#define HAS_USB 1 +#define HAS_BEEP 1 +#define BEEPER_TIMER 2 +#define BEEPER_CHANNEL 3 +#define BEEPER_PORT (&stm_gpioa) +#define BEEPER_PIN 2 +#define HAS_BATTERY_REPORT 1 +#define HAS_RADIO 0 +#define HAS_TELEMETRY 0 +#define HAS_APRS 0 +#define HAS_COMPANION 1 + +#define HAS_SPI_1 1 +#define SPI_1_PA5_PA6_PA7 1 /* Barometer */ +#define SPI_1_PB3_PB4_PB5 1 /* Accelerometer */ +#define SPI_1_PE13_PE14_PE15 0 +#define SPI_1_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_SPI_2 1 +#define SPI_2_PB13_PB14_PB15 1 /* Flash, IMU, Companion */ +#define SPI_2_PD1_PD3_PD4 0 +#define SPI_2_OSPEEDR STM_OSPEEDR_10MHz + +#define HAS_I2C_1 1 +#define I2C_1_PB8_PB9 1 + +#define HAS_I2C_2 0 +#define I2C_2_PB10_PB11 0 + +#define PACKET_HAS_SLAVE 0 +#define PACKET_HAS_MASTER 0 + +#define LOW_LEVEL_DEBUG 0 + +#define LED_PORT_ENABLE STM_RCC_AHBENR_GPIOAEN +#define LED_PORT (&stm_gpioa) +#define LED_PIN_RED 9 +#define LED_PIN_GREEN 10 +#define AO_LED_RED (1 << LED_PIN_RED) +#define AO_LED_GREEN (1 << LED_PIN_GREEN) + +#define LEDS_AVAILABLE (AO_LED_RED | AO_LED_GREEN) + +#define HAS_GPS 0 +#define HAS_FLIGHT 1 +#define HAS_ADC 1 +#define HAS_ADC_TEMP 1 +#define HAS_LOG 1 + +/* + * Igniter + */ + +#define HAS_IGNITE 1 +#define HAS_IGNITE_REPORT 1 + +#define AO_SENSE_PYRO(p,n) ((p)->adc.sense[n]) +#define AO_SENSE_DROGUE(p) ((p)->adc.sense[4]) +#define AO_SENSE_MAIN(p) ((p)->adc.sense[5]) +#define AO_IGNITER_CLOSED 400 +#define AO_IGNITER_OPEN 60 + +/* Pyro A */ +#define AO_PYRO_PORT_0 (&stm_gpioa) +#define AO_PYRO_PIN_0 15 + +/* Pyro B */ +#define AO_PYRO_PORT_1 (&stm_gpioc) +#define AO_PYRO_PIN_1 10 + +/* Pyro C */ +#define AO_PYRO_PORT_2 (&stm_gpiob) +#define AO_PYRO_PIN_2 11 + +/* Pyro D */ +#define AO_PYRO_PORT_3 (&stm_gpiob) +#define AO_PYRO_PIN_3 10 + +/* Drogue */ +#define AO_IGNITER_DROGUE_PORT (&stm_gpioa) +#define AO_IGNITER_DROGUE_PIN 0 + +/* Main */ +#define AO_IGNITER_MAIN_PORT (&stm_gpioa) +#define AO_IGNITER_MAIN_PIN 1 + +/* Number of general purpose pyro channels available */ +#define AO_PYRO_NUM 4 + +/* + * ADC + */ +#define AO_DATA_RING 32 +#define AO_ADC_NUM_SENSE 6 + +struct ao_adc { + int16_t sense[AO_ADC_NUM_SENSE]; + int16_t v_batt; + int16_t v_pbatt; + int16_t temp; +}; + +#define AO_ADC_DUMP(p) \ + printf("tick: %5u A: %5d B: %5d C: %5d D: %5d drogue: %5d main: %5d batt: %5d pbatt: %5d temp: %5d\n", \ + (p)->tick, \ + (p)->adc.sense[0], (p)->adc.sense[1], (p)->adc.sense[2], \ + (p)->adc.sense[3], (p)->adc.sense[4], (p)->adc.sense[5], \ + (p)->adc.v_batt, (p)->adc.v_pbatt, (p)->adc.temp) + +#define AO_ADC_SENSE_A 14 +#define AO_ADC_SENSE_A_PORT (&stm_gpioc) +#define AO_ADC_SENSE_A_PIN 4 + +#define AO_ADC_SENSE_B 15 +#define AO_ADC_SENSE_B_PORT (&stm_gpioc) +#define AO_ADC_SENSE_B_PIN 5 + +#define AO_ADC_SENSE_C 13 +#define AO_ADC_SENSE_C_PORT (&stm_gpioc) +#define AO_ADC_SENSE_C_PIN 3 + +#define AO_ADC_SENSE_D 12 +#define AO_ADC_SENSE_D_PORT (&stm_gpioc) +#define AO_ADC_SENSE_D_PIN 2 + +#define AO_ADC_SENSE_DROGUE 11 +#define AO_ADC_SENSE_DROGUE_PORT (&stm_gpioc) +#define AO_ADC_SENSE_DROGUE_PIN 1 + +#define AO_ADC_SENSE_MAIN 10 +#define AO_ADC_SENSE_MAIN_PORT (&stm_gpioc) +#define AO_ADC_SENSE_MAIN_PIN 0 + +#define AO_ADC_V_BATT 8 +#define AO_ADC_V_BATT_PORT (&stm_gpiob) +#define AO_ADC_V_BATT_PIN 0 + +#define AO_ADC_V_PBATT 9 +#define AO_ADC_V_PBATT_PORT (&stm_gpiob) +#define AO_ADC_V_PBATT_PIN 1 + +#define AO_ADC_TEMP 16 + +#define AO_ADC_RCC_AHBENR ((1 << STM_RCC_AHBENR_GPIOAEN) | \ + (1 << STM_RCC_AHBENR_GPIOEEN) | \ + (1 << STM_RCC_AHBENR_GPIOBEN)) + +#define AO_NUM_ADC_PIN (AO_ADC_NUM_SENSE + 2) + +#define AO_ADC_PIN0_PORT AO_ADC_SENSE_A_PORT +#define AO_ADC_PIN0_PIN AO_ADC_SENSE_A_PIN +#define AO_ADC_PIN1_PORT AO_ADC_SENSE_B_PORT +#define AO_ADC_PIN1_PIN AO_ADC_SENSE_B_PIN +#define AO_ADC_PIN2_PORT AO_ADC_SENSE_C_PORT +#define AO_ADC_PIN2_PIN AO_ADC_SENSE_C_PIN +#define AO_ADC_PIN3_PORT AO_ADC_SENSE_D_PORT +#define AO_ADC_PIN3_PIN AO_ADC_SENSE_D_PIN +#define AO_ADC_PIN4_PORT AO_ADC_SENSE_DROGUE_PORT +#define AO_ADC_PIN4_PIN AO_ADC_SENSE_DROGUE_PIN +#define AO_ADC_PIN5_PORT AO_ADC_SENSE_MAIN_PORT +#define AO_ADC_PIN5_PIN AO_ADC_SENSE_MAIN_PIN +#define AO_ADC_PIN6_PORT AO_ADC_V_BATT_PORT +#define AO_ADC_PIN6_PIN AO_ADC_V_BATT_PIN +#define AO_ADC_PIN7_PORT AO_ADC_V_PBATT_PORT +#define AO_ADC_PIN7_PIN AO_ADC_V_PBATT_PIN + +#define AO_NUM_ADC (AO_ADC_NUM_SENSE + 3) + +#define AO_ADC_SQ1 AO_ADC_SENSE_A +#define AO_ADC_SQ2 AO_ADC_SENSE_B +#define AO_ADC_SQ3 AO_ADC_SENSE_C +#define AO_ADC_SQ4 AO_ADC_SENSE_D +#define AO_ADC_SQ5 AO_ADC_SENSE_DROGUE +#define AO_ADC_SQ6 AO_ADC_SENSE_MAIN +#define AO_ADC_SQ7 AO_ADC_V_BATT +#define AO_ADC_SQ8 AO_ADC_V_PBATT +#define AO_ADC_SQ9 AO_ADC_TEMP + +/* + * Voltage divider on ADC battery sampler + */ +#define AO_BATTERY_DIV_PLUS 56 /* 5.6k */ +#define AO_BATTERY_DIV_MINUS 100 /* 10k */ + +/* + * Voltage divider on ADC igniter samplers + */ +#define AO_IGNITE_DIV_PLUS 100 /* 100k */ +#define AO_IGNITE_DIV_MINUS 27 /* 27k */ + +/* + * ADC reference in decivolts + */ +#define AO_ADC_REFERENCE_DV 33 + +/* + * Pressure sensor settings + */ +#define HAS_MS5607 1 +#define HAS_MS5611 0 +#define AO_MS5607_PRIVATE_PINS 1 +#define AO_MS5607_CS_PORT (&stm_gpioa) +#define AO_MS5607_CS_PIN 3 +#define AO_MS5607_CS_MASK (1 << AO_MS5607_CS_PIN) +#define AO_MS5607_MISO_PORT (&stm_gpioa) +#define AO_MS5607_MISO_PIN 6 +#define AO_MS5607_MISO_MASK (1 << AO_MS5607_MISO_PIN) +#define AO_MS5607_SPI_INDEX AO_SPI_1_PA5_PA6_PA7 + +/* + * SPI Flash memory + */ + +#define M25_MAX_CHIPS 1 +#define AO_M25_SPI_CS_PORT (&stm_gpiob) +#define AO_M25_SPI_CS_PIN 12 +#define AO_M25_SPI_CS_MASK (1 << AO_M25_SPI_CS_PIN) +#define AO_M25_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* + * bmx160 + */ + +#define HAS_BMX160 1 +#define AO_BMX160_INT_PORT (&stm_gpioc) +#define AO_BMX160_INT_PIN 15 +#define AO_BMX160_SPI_BUS (AO_SPI_2_PB13_PB14_PB15 | AO_SPI_MODE_0) +#define AO_BMX160_SPI_CS_PORT (&stm_gpioc) +#define AO_BMX160_SPI_CS_PIN 13 +#define HAS_IMU 1 + +#define ao_data_along(packet) ((packet)->bmx160.acc_x) +#define ao_data_across(packet) (-(packet)->bmx160.acc_y) +#define ao_data_through(packet) ((packet)->bmx160.acc_z) + +#define ao_data_roll(packet) ((packet)->bmx160.gyr_x) +#define ao_data_pitch(packet) (-(packet)->bmx160.gyr_y) +#define ao_data_yaw(packet) ((packet)->bmx160.gyr_z) + +#define ao_data_mag_along(packet) ((packet)->bmx160.mag_x) +#define ao_data_mag_across(packet) (-(packet)->bmx160.mag_y) +#define ao_data_mag_through(packet) ((packet)->bmx160.mag_z) + +/* ADXL375 */ + +#define HAS_ADXL375 1 +#define AO_ADXL375_SPI_INDEX (AO_SPI_1_PB3_PB4_PB5 | AO_SPI_MODE_3) +#define AO_ADXL375_CS_PORT (&stm_gpioc) +#define AO_ADXL375_CS_PIN 12 +#define AO_ADXL375_SPI_SPEED AO_SPI_SPEED_4MHz + +#define AO_ADXL375_INT1_PORT (&stm_gpiob) +#define AO_ADXL375_INT1_PIN 8 + +#define AO_ADXL375_INT2_PORT (&stm_gpiob) +#define AO_ADXL375_INT2_PIN 9 + +#define AO_ADXL375_AXIS x +#define AO_ADXL375_INVERT 1 + +#define NUM_CMDS 16 + +/* + * Companion + */ + +#define AO_COMPANION_CS_PORT (&stm_gpiob) +#define AO_COMPANION_CS_PIN (6) +#define AO_COMPANION_SPI_BUS AO_SPI_2_PB13_PB14_PB15 + +/* + * Monitor + */ + +#define HAS_MONITOR 0 +#define LEGACY_MONITOR 0 +#define HAS_MONITOR_PUT 0 +#define AO_MONITOR_LED 0 +#define HAS_RSSI 0 + +/* + * Profiling Viterbi decoding + */ + +#ifndef AO_PROFILE +#define AO_PROFILE 0 +#endif + +#endif /* _AO_PINS_H_ */ diff --git a/src/easymega-v3.0/flash-loader/Makefile b/src/easymega-v3.0/flash-loader/Makefile new file mode 100644 index 00000000..2b715caf --- /dev/null +++ b/src/easymega-v3.0/flash-loader/Makefile @@ -0,0 +1,8 @@ +# +# AltOS flash loader build +# +# + +TOPDIR=../.. +HARDWARE=easymega-v3.0 +include $(TOPDIR)/stm/Makefile-flash.defs diff --git a/src/easymega-v3.0/flash-loader/ao_pins.h b/src/easymega-v3.0/flash-loader/ao_pins.h new file mode 100644 index 00000000..324b0eb9 --- /dev/null +++ b/src/easymega-v3.0/flash-loader/ao_pins.h @@ -0,0 +1,35 @@ +/* + * Copyright © 2018 Bdale Garbee <bdale@gag.com> + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. + */ + +#ifndef _AO_PINS_H_ +#define _AO_PINS_H_ + +/* External crystal at 16MHz */ +#define AO_HSE 16000000 + +#include <ao_flash_stm_pins.h> + +/* Companion port cs_companion0 PB6 */ + +#define AO_BOOT_PIN 1 +#define AO_BOOT_APPLICATION_GPIO stm_gpiob +#define AO_BOOT_APPLICATION_PIN 6 +#define AO_BOOT_APPLICATION_VALUE 1 +#define AO_BOOT_APPLICATION_MODE AO_EXTI_MODE_PULL_UP + +#endif /* _AO_PINS_H_ */ diff --git a/src/kernel/ao.h b/src/kernel/ao.h index dddcd9cb..fb7af24d 100644 --- a/src/kernel/ao.h +++ b/src/kernel/ao.h @@ -75,6 +75,7 @@ typedef AO_PORT_TYPE ao_port_t; #define AO_PANIC_SELF_TEST_HMC5883 0x40 | 2 /* Self test failure */ #define AO_PANIC_SELF_TEST_MPU6000 0x40 | 3 /* Self test failure */ #define AO_PANIC_SELF_TEST_MPU9250 0x40 | 3 /* Self test failure */ +#define AO_PANIC_SELF_TEST_BMX160 0x40 | 3 /* Self test failure */ #define AO_PANIC_SELF_TEST_MS5607 0x40 | 4 /* Self test failure */ #define AO_PANIC_SELF_TEST_ADS124S0X 0x40 | 5 /* Self test failure */ @@ -937,12 +938,6 @@ ao_log_single(void); #define AO_TELEPYRO_NUM_ADC 9 -#ifndef ao_xmemcpy -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) -#endif - /* * ao_terraui.c */ diff --git a/src/kernel/ao_config.c b/src/kernel/ao_config.c index f7c79e7d..9f329f17 100644 --- a/src/kernel/ao_config.c +++ b/src/kernel/ao_config.c @@ -138,8 +138,8 @@ _ao_config_get(void) /* Version 0 stuff */ ao_config.main_deploy = AO_CONFIG_DEFAULT_MAIN_DEPLOY; - ao_xmemset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); - ao_xmemcpy(&ao_config.callsign, AO_CONFIG_DEFAULT_CALLSIGN, + memset(&ao_config.callsign, '\0', sizeof (ao_config.callsign)); + memcpy(&ao_config.callsign, AO_CONFIG_DEFAULT_CALLSIGN, sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); ao_config._legacy_radio_channel = 0; } @@ -171,7 +171,7 @@ _ao_config_get(void) if (minor < 8) ao_config.radio_enable = AO_RADIO_ENABLE_CORE; if (minor < 9) - ao_xmemset(&ao_config.aes_key, '\0', AO_AES_LEN); + memset(&ao_config.aes_key, '\0', AO_AES_LEN); if (minor < 10) ao_config.frequency = 434550 + ao_config._legacy_radio_channel * 100; if (minor < 11) @@ -250,7 +250,7 @@ _ao_config_get(void) #if HAS_RADIO_RATE ao_config.radio_rate = AO_CONFIG_DEFAULT_RADIO_RATE; #endif - ao_xmemcpy(&ao_config.callsign, AO_CONFIG_DEFAULT_CALLSIGN, + memcpy(&ao_config.callsign, AO_CONFIG_DEFAULT_CALLSIGN, sizeof(AO_CONFIG_DEFAULT_CALLSIGN) - 1); } #endif @@ -294,7 +294,7 @@ ao_config_callsign_set(void) uint8_t c; static char callsign[AO_MAX_CALLSIGN + 1]; - ao_xmemset(callsign, '\0', sizeof callsign); + memset(callsign, '\0', sizeof callsign); ao_cmd_white(); c = 0; while (ao_cmd_lex_c != '\n') { @@ -307,7 +307,7 @@ ao_config_callsign_set(void) if (ao_cmd_status != ao_cmd_success) return; _ao_config_edit_start(); - ao_xmemcpy(&ao_config.callsign, &callsign, + memcpy(&ao_config.callsign, &callsign, AO_MAX_CALLSIGN + 1); _ao_config_edit_finish(); } diff --git a/src/kernel/ao_data.h b/src/kernel/ao_data.h index 4fc9db8f..a7e58762 100644 --- a/src/kernel/ao_data.h +++ b/src/kernel/ao_data.h @@ -76,6 +76,13 @@ #define AO_DATA_MAX6691 0 #endif +#if HAS_BMX160 +#include <ao_bmx160.h> +#define AO_DATA_BMX160 (1 << 2) +#else +#define AO_DATA_BMX160 0 +#endif + #ifndef HAS_SENSOR_ERRORS #if HAS_IMU || HAS_MMA655X || HAS_MS5607 || HAS_MS5611 #define HAS_SENSOR_ERRORS 1 @@ -88,7 +95,7 @@ extern uint8_t ao_sensor_errors; #ifdef AO_DATA_RING -#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X|AO_DATA_MPU9250|AO_DATA_ADXL375) +#define AO_DATA_ALL (AO_DATA_ADC|AO_DATA_MS5607|AO_DATA_MPU6000|AO_DATA_HMC5883|AO_DATA_MMA655X|AO_DATA_MPU9250|AO_DATA_ADXL375|AO_DATA_BMX160) struct ao_data { uint16_t tick; @@ -123,6 +130,9 @@ struct ao_data { #if HAS_ADS131A0X struct ao_ads131a0x_sample ads131a0x; #endif +#if HAS_BMX160 + struct ao_bmx160_sample bmx160; +#endif }; #define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1)) @@ -431,6 +441,29 @@ static inline float ao_convert_accel(int16_t sensor) #endif +#if !HAS_GYRO && HAS_BMX160 + +#define HAS_GYRO 1 + +typedef int16_t gyro_t; /* in raw sample units */ +typedef int16_t angle_t; /* in degrees */ + +/* Y axis is aligned with the direction of motion (along) */ +/* X axis is aligned in the other board axis (across) */ +/* Z axis is aligned perpendicular to the board (through) */ + +static inline float ao_convert_gyro(float sensor) +{ + return ao_bmx160_gyro(sensor); +} + +static inline float ao_convert_accel(int16_t sensor) +{ + return ao_bmx160_accel(sensor); +} + +#endif + #if !HAS_MAG && HAS_HMC5883 #define HAS_MAG 1 diff --git a/src/kernel/ao_ee_fake.c b/src/kernel/ao_ee_fake.c index cef0a438..f5e195ad 100644 --- a/src/kernel/ao_ee_fake.c +++ b/src/kernel/ao_ee_fake.c @@ -33,6 +33,6 @@ ao_ee_write_config(uint8_t *buf, uint16_t len) uint8_t ao_ee_read_config(uint8_t *buf, uint16_t len) { - ao_xmemset(buf, '\0', len); + memset(buf, '\0', len); return 1; } diff --git a/src/kernel/ao_gps_report.c b/src/kernel/ao_gps_report.c index 0ef9a725..01226ba5 100644 --- a/src/kernel/ao_gps_report.c +++ b/src/kernel/ao_gps_report.c @@ -32,9 +32,9 @@ ao_gps_report(void) ao_sleep(&ao_gps_new); ao_mutex_get(&ao_gps_mutex); if (new & AO_GPS_NEW_DATA) - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + memcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); if (new & AO_GPS_NEW_TRACKING) - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); diff --git a/src/kernel/ao_gps_report_mega.c b/src/kernel/ao_gps_report_mega.c index 37f1beaa..0cba972b 100644 --- a/src/kernel/ao_gps_report_mega.c +++ b/src/kernel/ao_gps_report_mega.c @@ -70,9 +70,9 @@ ao_gps_report_mega(void) ao_sleep(&ao_gps_new); ao_mutex_get(&ao_gps_mutex); if (new & AO_GPS_NEW_DATA) - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + memcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); if (new & AO_GPS_NEW_TRACKING) - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); diff --git a/src/kernel/ao_gps_report_metrum.c b/src/kernel/ao_gps_report_metrum.c index e5839b36..b5f58893 100644 --- a/src/kernel/ao_gps_report_metrum.c +++ b/src/kernel/ao_gps_report_metrum.c @@ -34,9 +34,9 @@ ao_gps_report_metrum(void) ao_sleep(&ao_gps_new); ao_mutex_get(&ao_gps_mutex); if (new & AO_GPS_NEW_DATA) - ao_xmemcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); + memcpy(&gps_data, &ao_gps_data, sizeof (ao_gps_data)); if (new & AO_GPS_NEW_TRACKING) - ao_xmemcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); + memcpy(&gps_tracking_data, &ao_gps_tracking_data, sizeof (ao_gps_tracking_data)); ao_gps_new = 0; ao_mutex_put(&ao_gps_mutex); diff --git a/src/kernel/ao_host.h b/src/kernel/ao_host.h index 592de54c..f3221822 100644 --- a/src/kernel/ao_host.h +++ b/src/kernel/ao_host.h @@ -127,6 +127,6 @@ struct ao_config { struct ao_config ao_config = { 250, 16000 }; -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) +#define memcpy(d,s,c) memcpy(d,s,c) +#define memset(d,v,c) memset(d,v,c) +#define memcmp(d,s,c) memcmp(d,s,c) diff --git a/src/kernel/ao_log_telem.c b/src/kernel/ao_log_telem.c index 8c0b88e4..f985417a 100644 --- a/src/kernel/ao_log_telem.c +++ b/src/kernel/ao_log_telem.c @@ -103,7 +103,7 @@ ao_log_single(void) while (ao_log_running) { /* Write samples to EEPROM */ while (ao_log_monitor_pos != ao_monitor_head) { - ao_xmemcpy(&ao_log_single_write_data.telemetry, + memcpy(&ao_log_single_write_data.telemetry, &ao_monitor_ring[ao_log_monitor_pos], AO_LOG_SINGLE_SIZE); ao_log_single_write(); diff --git a/src/kernel/ao_monitor.c b/src/kernel/ao_monitor.c index 400ae4df..cd511699 100644 --- a/src/kernel/ao_monitor.c +++ b/src/kernel/ao_monitor.c @@ -157,7 +157,7 @@ ao_monitor_put(void) state = recv_orig.telemetry_orig.flight_state; rssi = (int16_t) AO_RSSI_FROM_RADIO(recv_orig.rssi); - ao_xmemcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); + memcpy(callsign, recv_orig.telemetry_orig.callsign, AO_MAX_CALLSIGN); if (state > ao_flight_invalid) state = ao_flight_invalid; if (recv_orig.status & PKT_APPEND_STATUS_1_CRC_OK) { diff --git a/src/kernel/ao_radio_cmac.c b/src/kernel/ao_radio_cmac.c index 155fce35..c7b48e06 100644 --- a/src/kernel/ao_radio_cmac.c +++ b/src/kernel/ao_radio_cmac.c @@ -132,7 +132,7 @@ ao_radio_cmac_send(void *packet, uint8_t len) if (len > AO_CMAC_MAX_LEN) return AO_RADIO_CMAC_LEN_ERROR; ao_mutex_get(&ao_radio_cmac_mutex); - ao_xmemcpy(cmac_data, packet, len); + memcpy(cmac_data, packet, len); #if AO_LED_TX ao_led_on(AO_LED_TX); #endif @@ -159,7 +159,7 @@ ao_radio_cmac_recv(void *packet, uint8_t len, uint16_t timeout) ao_led_off(AO_LED_RX); #endif if (i == AO_RADIO_CMAC_OK) - ao_xmemcpy(packet, cmac_data, len); + memcpy(packet, cmac_data, len); ao_mutex_put(&ao_radio_cmac_mutex); return i; } diff --git a/src/kernel/ao_telemetry.c b/src/kernel/ao_telemetry.c index ae33173f..99fc4534 100644 --- a/src/kernel/ao_telemetry.c +++ b/src/kernel/ao_telemetry.c @@ -333,12 +333,12 @@ ao_send_configuration(void) #endif telemetry.configuration.flight_log_max = ao_config.flight_log_max >> 10; - ao_xmemcpy (telemetry.configuration.callsign, - ao_config.callsign, - AO_MAX_CALLSIGN); - ao_xmemcpy (telemetry.configuration.version, - ao_version, - AO_MAX_VERSION); + memcpy (telemetry.configuration.callsign, + ao_config.callsign, + AO_MAX_CALLSIGN); + memcpy (telemetry.configuration.version, + ao_version, + AO_MAX_VERSION); ao_telemetry_config_cur = ao_telemetry_config_max; ao_telemetry_send(); } @@ -350,6 +350,18 @@ static int8_t ao_telemetry_gps_max; static int8_t ao_telemetry_loc_cur; static int8_t ao_telemetry_sat_cur; +static inline void * +telemetry_bits(struct ao_telemetry_location *l) +{ + return ((uint8_t *) l) + offsetof(struct ao_telemetry_location, flags); +} + +static inline int +telemetry_size(void) +{ + return sizeof(struct ao_telemetry_location) - offsetof(struct ao_telemetry_location, flags); +} + static void ao_send_location(void) { @@ -357,9 +369,9 @@ ao_send_location(void) { telemetry.generic.type = AO_TELEMETRY_LOCATION; ao_mutex_get(&ao_gps_mutex); - ao_xmemcpy(&telemetry.location.flags, - &ao_gps_data.flags, - 27); + memcpy(telemetry_bits(&telemetry.location), + telemetry_bits(&ao_gps_data), + telemetry_size()); telemetry.location.tick = ao_gps_tick; ao_mutex_put(&ao_gps_mutex); ao_telemetry_loc_cur = ao_telemetry_gps_max; @@ -375,7 +387,7 @@ ao_send_satellite(void) telemetry.generic.type = AO_TELEMETRY_SATELLITE; ao_mutex_get(&ao_gps_mutex); telemetry.satellite.channels = ao_gps_tracking_data.channels; - ao_xmemcpy(&telemetry.satellite.sats, + memcpy(&telemetry.satellite.sats, &ao_gps_tracking_data.sats, AO_MAX_GPS_TRACKING * sizeof (struct ao_telemetry_satellite_info)); ao_mutex_put(&ao_gps_mutex); @@ -399,7 +411,7 @@ ao_send_companion(void) telemetry.companion.update_period = ao_companion_setup.update_period; telemetry.companion.channels = ao_companion_setup.channels; ao_mutex_get(&ao_companion_mutex); - ao_xmemcpy(&telemetry.companion.companion_data, + memcpy(&telemetry.companion.companion_data, ao_companion_data, ao_companion_setup.channels * 2); ao_mutex_put(&ao_companion_mutex); diff --git a/src/product/ao_terraui.c b/src/product/ao_terraui.c index 3a989f5a..102abb91 100644 --- a/src/product/ao_terraui.c +++ b/src/product/ao_terraui.c @@ -598,7 +598,7 @@ ao_terramonitor(void) case AO_TELEMETRY_SENSOR_TELEMETRUM: case AO_TELEMETRY_SENSOR_TELEMINI: case AO_TELEMETRY_SENSOR_TELENANO: - ao_xmemcpy(&ao_tel_sensor, &ao_monitor_ring[monitor], sizeof (ao_tel_sensor)); + memcpy(&ao_tel_sensor, &ao_monitor_ring[monitor], sizeof (ao_tel_sensor)); if (ao_tel_sensor.state < ao_flight_boost) { ao_tel_max_speed = 0; ao_tel_max_height = 0; @@ -611,10 +611,10 @@ ao_terramonitor(void) ao_telem_progress = (ao_telem_progress + 1) & 0x3; break; case AO_TELEMETRY_LOCATION: - ao_xmemcpy(&ao_tel_location, &ao_monitor_ring[monitor], sizeof (ao_tel_location)); + memcpy(&ao_tel_location, &ao_monitor_ring[monitor], sizeof (ao_tel_location)); break; case AO_TELEMETRY_CONFIGURATION: - ao_xmemcpy(&ao_tel_config, &ao_monitor_ring[monitor], sizeof (ao_tel_config)); + memcpy(&ao_tel_config, &ao_monitor_ring[monitor], sizeof (ao_tel_config)); } } } diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c index 6d007575..35e0da3f 100644 --- a/src/test/ao_flight_test.c +++ b/src/test/ao_flight_test.c @@ -331,10 +331,6 @@ struct ao_cmds { const char *help; }; -#define ao_xmemcpy(d,s,c) memcpy(d,s,c) -#define ao_xmemset(d,v,c) memset(d,v,c) -#define ao_xmemcmp(d,s,c) memcmp(d,s,c) - #define AO_NEED_ALTITUDE_TO_PRES 1 #if TELEMEGA || TELEMETRUM_V2 || EASYMINI #include "ao_convert_pa.c" @@ -358,9 +354,6 @@ struct ao_ms5607_prom ao_ms5607_prom; struct ao_config ao_config; -#define x (x) - - extern int16_t ao_ground_accel, ao_flight_accel; extern int16_t ao_accel_2g; |