Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Continuation of Matek M9NF4 GPS Uavcannode Board Support #19061

Merged
merged 8 commits into from
Feb 2, 2022

Conversation

davids5
Copy link
Member

@davids5 davids5 commented Jan 22, 2022

Continuation of #18621

Things to note:

  • Console is on TX1, TX2 pins.
  • Bootloader can be installed with STM32CubeProgrammer (bin file offset 0 will be loaded at 0x800 0000)
  • The board id is 1014. Therefore the bin file is found build dir .../deploy/1014.bin
  • It can be in stalled by coping it to the root of the SD card on the flight controller. (not this node the one running uavcan)
  • LED's share the JTAG pins - no LED indication of the bootloader state at the moment
  • ICM20602 is up and running but the Cannode does not publish it.
  • Top, sensors, urb, work_queue, i2cdetect, and reboot commands have been added
  • It is nor debug-able with Jtag and sems to be stable.

@dirksavage88 @ryanjAA - please give this a shot.

@ryanjAA
Copy link
Contributor

ryanjAA commented Jan 23, 2022

Thanks @davids5 - I just loaded the bootloader and updated the node firmware. A few tests:

GPS device shows it is there with listener sensor_gps from console on the FC and when connecting directly to the module over USB and watching the console via screen but I cant get satellites. I put the ardupilot ap_periph bootloader and related on and I get sats so it's not hardware or specific location.

Baro works but there is the temp error still (high number)

Mag doesnt show up as publishing data (did a calibrate for good measure). Went to the device directly to do a rm3100 status and it's not started so started it but it isn't on any of the busses still (tried all). Tested against ap_periph and mag does show up (so not hardware - that was my big concern)

Will go a few more checks on other things but great to see this continue. 👍

@dirksavage88 what do you see?

@davids5
Copy link
Member Author

davids5 commented Jan 23, 2022

Thanks @davids5 - I just loaded the bootloader and updated the node firmware. A few tests:

GPS device shows it is there with listener sensor_gps from console on the FC and when connecting directly to the module over USB and watching the console via screen but I cant get satellites. I put the ardupilot ap_periph bootloader and related on and I get sats so it's not hardware or specific location.

This could be noise on our SPI - I will try some adjustments

Baro works but there is the temp error still (high number)

My output on board looks ok temp is commensurate with other sensors - see below

Mag doesnt show up as publishing data (did a calibrate for good measure). Went to the device directly to do a rm3100 status and it's not started so started it but it isn't on any of the busses still (tried all). Tested against ap_periph and mag does show up (so not hardware - that was my big concern)

My output on board looks ok - see below

Will go a few more checks on other things but great to see this continue. 👍

@dirksavage88 what do you see?

Here is my whole context:
Hardware: Only Can and USB connected (JTAG and Console on TX1, RX1 - VTarget, GND and nRST is wired (green on back of board)
image

Whole setup: V5 running uavcan, CanBreakout
20220123_064426

Booted
image

Console Output from MATEK - via debug console on TX1,RX1

HW arch: MATEK_GNSS_M9N_F4
FW git-hash: 1e57169d9fca9c2d614c7acc270f24c5d3522602
FW version: 1.13.0 40 (17629248)
FW git-branch: pr-matek_m9nf4can
OS: NuttX
OS version: 0.0.0 0 (0)
OS git-hash: a4576ea73b36fc768d35ba9f86855ef4f9463498
Build datetime: Jan 22 2022 13:57:43
Build uri: localhost
Build variant: default
Toolchain: GNU GCC, 9.3.1 20200408 (release)
PX4GUID: 00010000000020383935563250110053004f
MCU: STM32F40x, rev. 1
nsh: mft: command not found
nsh: mft: command not found
Board architecture defaults: /etc/init.d/rc.board_arch_defaults
ERROR [param] Parameter EKF2_MULTI_IMU not found.
ERROR [param] Parameter EKF2_MULTI_MAG not found.
nsh: rgbled: command not found
nsh: rgbled_ncp5623c: command not found
Board sensors: /etc/init.d/rc.board_sensors
icm20602 #0 on SPI bus 1
rm3100 #0 on SPI bus 2
sercon: Registering CDC/ACM serial driver
sercon: Successfully registered the CDC/ACM serial driver
dps310 #0 on I2C bus 2 (external) address 0x76
ERROR [param] Parameter SENS_EN_PX4FLOW not found
INFO  [uavcannode] Node ID 125, bitrate 1000000

NuttShell (NSH) NuttX-10.2.0

nsh> work_queue status

Work Queue: 7  threads                          RATE        INTERVAL
|__ 1) wq:rate_ctrl
|   \__ 1) vehicle_angular_velocity         410.8 Hz         2434 us
|__ 2) wq:SPI1
|   \__ 1) icm20602                         400.1 Hz         2500 us
|__ 3) wq:SPI2
|   \__ 1) rm3100                            96.9 Hz        10324 us
|__ 4) wq:I2C2
|   \__ 1) dps310                            64.1 Hz        15610 us (15625 us)
|__ 5) wq:nav_and_controllers
|   |__ 1) sensors                          200.3 Hz         4993 us
|   |__ 2) vehicle_acceleration             208.5 Hz         4796 us
|   |__ 3) vehicle_air_data                  20.9 Hz        47789 us
|   |__ 4) vehicle_gps_position               7.3 Hz       136586 us
|   \__ 5) vehicle_magnetometer              96.9 Hz        10324 us
|__ 6) wq:INS0
|   \__ 1) vehicle_imu                      200.3 Hz         4992 us
\__ 7) wq:uavcan
    \__ 1) uavcannode                       429.5 Hz         2328 us (10000 us)

nsh> sensors status
selected gyro: 3670026 (0)
validator: best: 0, prev best: 0, failsafe: NO (0 events)
sensor #0, prio: 50, state: OK
        val:   0.0004, lp:   0.0013 mean dev:   0.0000 RMS:   0.0012 conf:   1.0000
        val:   0.0023, lp:   0.0046 mean dev:   0.0000 RMS:   0.0017 conf:   1.0000
        val:  -0.0011, lp:  -0.0014 mean dev:  -0.0000 RMS:   0.0006 conf:   1.0000

selected accel: 3670026 (0)
validator: best: 0, prev best: 0, failsafe: NO (0 events)
sensor #0, prio: 50, state: OK
        val:  -0.3591, lp:  -0.3822 mean dev:  -0.0001 RMS:   0.0111 conf:   1.0000
        val:   0.1863, lp:   0.2029 mean dev:  -0.0000 RMS:   0.0100 conf:   1.0000
        val:  -9.9338, lp:  -9.8781 mean dev:   0.0003 RMS:   0.0221 conf:   1.0000

[vehicle_magnetometer] selected magnetometer: 458770 (0)
validator: best: 0, prev best: 0, failsafe: NO (0 events)
sensor #0, prio: 50, state: OK
        val:   0.1397, lp:   0.1395 mean dev:   0.0000 RMS:   0.0012 conf:   1.0000
        val:  -0.1009, lp:  -0.1009 mean dev:   0.0000 RMS:   0.0004 conf:   1.0000
        val:   0.3488, lp:   0.3484 mean dev:  -0.0000 RMS:   0.0006 conf:   1.0000
MAG 458770 EN: 1, offset: [0.000 0.000 0.000], scale: [1.000 1.000 1.000], nan degC, Internal

[vehicle_air_data] selected barometer: 6845969 (0)
validator: best: 0, prev best: 0, failsafe: NO (0 events)
sensor #0, prio: 0, state: OK
        val: 101360.2734, lp: 101359.9609 mean dev:   0.0028 RMS:   0.3091 conf:   1.0000
        val:  38.5021, lp:  38.3987 mean dev:   0.1203 RMS:   0.0428 conf:   1.0000
        val:   0.0000, lp:   0.0000 mean dev:   0.0000 RMS:   0.0000 conf:   1.0000

Airspeed status:
        no data

[vehicle_acceleration] selected sensor: 3670026, rate: 400.8 Hz, estimated bias: [0.0000 0.0000 0.0000]
ACC 3670026 EN: 1, offset: [0.000 0.000 0.000], scale: [1.000 1.000 1.000], nan degC, Internal

[vehicle_angular_velocity] selected sensor: 3670026, rate: 8017.0 Hz FIFO, estimated bias: [0.00000 0.00000 0.00000]
GYRO 3670026 EN: 1, offset: [0.000 0.000 0.000], nan degC, Internal
vehicle_angular_velocity: gyro filter: 10365 events, 438104us elapsed, 42.27us avg, min 21us max 684us 6.608us rms
vehicle_angular_velocity: gyro filter reset: 1 events
vehicle_angular_velocity: gyro selection changed: 1 events

[vehicle_imu] 0 - Accel: 3670026, interval: 2494.7 us (SD 0.6 us), Gyro: 3670026, interval: 2494.7 us (SD 0.6 us)
vehicle_imu: accel data gap: 1 events
vehicle_imu: gyro data gap: 1 events
ACC 3670026 EN: 1, offset: [0.000 0.000 0.000], scale: [1.000 1.000 1.000], nan degC, Internal
GYRO 3670026 EN: 1, offset: [0.000 0.000 0.000], nan degC, Internal

nsh> gps status
INFO  [gps] Main GPS
INFO  [gps] protocol: UBX
INFO  [gps] status: OK, port: /dev/ttyS2, baudrate: 115200
INFO  [gps] sat info: disabled
INFO  [gps] rate reading:                 1103 B/s
INFO  [gps] rate position:                8.02 Hz
INFO  [gps] rate velocity:                8.02 Hz
INFO  [gps] rate publication:             8.02 Hz
INFO  [gps] rate RTCM injection:          0.00 Hz
 sensor_gps
    timestamp: 784481176 (0.007782 seconds ago)
    time_utc_usec: 0
    device_id: 11075605 (Type: 0xA9, SERIAL:2 (0x00))
    lat: 0
    lon: 0
    alt: -17000
    alt_ellipsoid: 0
    s_variance_m_s: 999.0001
    c_variance_rad: 3.1416
    eph: 4294967.5000
    epv: 3985693.2500
    hdop: 99.9900
    vdop: 99.9900
    noise_per_ms: 95
    jamming_indicator: 15
    vel_m_s: 0.0000
    vel_n_m_s: 0.0000
    vel_e_m_s: 0.0000
    vel_d_m_s: 0.0000
    cog_rad: 0.0000
    timestamp_time_relative: 0
    heading: nan
    heading_offset: 0.0000
    heading_accuracy: 0.0000
    automatic_gain_control: 3510
    fix_type: 0
    jamming_state: 0
    vel_ned_valid: False
    satellites_used: 0

nsh> icm20602 status
INFO  [SPI_I2C] Running on SPI Bus 1
INFO  [icm20602] FIFO empty interval: 2500 us (400.0 Hz)
icm20602: bad register: 0 events
icm20602: bad transfer: 0 events
icm20602: FIFO empty: 0 events
icm20602: FIFO overflow: 0 events
icm20602: FIFO reset: 1 events
icm20602: DRDY missed: 0 events

nsh> rm3100 status
INFO  [SPI_I2C] Running on SPI Bus 2
rm3100: read: 75970 events, 14309743us elapsed, 188.36us avg, min 133us max 892us 188.812us rms
rm3100: comms_errors: 0 events
INFO  [rm3100] poll interval:  10000

nsh> dps310 status
INFO  [SPI_I2C] Running on I2C Bus 2, Address 0x76
dps310: read: 50208 events, 40740777us elapsed, 811.44us avg, min 408us max 2254us 513.998us rms
dps310: comm errors: 0 events

nsh>  listener sensor_mag

TOPIC: sensor_mag
 sensor_mag
    timestamp: 980472255 (0.003355 seconds ago)
    timestamp_sample: 980472120 (135 us before timestamp)
    device_id: 458770 (Type: 0x07, SPI:2 (0x00))
    x: 0.1426
    y: -0.0806
    z: 0.3809
    temperature: nan
    error_count: 0

nsh>  listener sensor_accel

TOPIC: sensor_accel
 sensor_accel
    timestamp: 980479392 (0.001321 seconds ago)
    timestamp_sample: 980478673 (719 us before timestamp)
    device_id: 3670026 (Type: 0x38, SPI:1 (0x00))
    x: -0.5205
    y: 0.1216
    z: -9.9984
    temperature: 41.6536
    error_count: 0
    clip_counter: [0, 0, 0]
    samples: 10

nsh>  listener sensor_gyro

TOPIC: sensor_gyro
 sensor_gyro
    timestamp: 980511814 (0.001034 seconds ago)
    timestamp_sample: 980511113 (701 us before timestamp)
    device_id: 3670026 (Type: 0x38, SPI:1 (0x00))
    x: 0.0057
    y: 0.0031
    z: 0.0006
    temperature: 41.6438
    error_count: 0
    samples: 20

nsh>  listener sensor_baro

TOPIC: sensor_baro
 sensor_baro
    timestamp: 980532687 (0.009321 seconds ago)
    timestamp_sample: 980531827 (860 us before timestamp)
    device_id: 6845969 (Type: 0x68, I2C:2 (0x76))
    error_count: 0
    pressure: 1013.6528
    temperature: 43.8234
nsh>  listener sensor_gps

TOPIC: sensor_gps
 sensor_gps
    timestamp: 980436096 (0.133194 seconds ago)
    time_utc_usec: 0
    device_id: 11075605 (Type: 0xA9, SERIAL:2 (0x00))
    lat: 0
    lon: 0
    alt: -17000
    alt_ellipsoid: 0
    s_variance_m_s: 999.0001
    c_variance_rad: 3.1416
    eph: 4294967.5000
    epv: 4056037.0000
    hdop: 99.9900
    vdop: 99.9900
    noise_per_ms: 93
    jamming_indicator: 12
    vel_m_s: 0.0000
    vel_n_m_s: 0.0000
    vel_e_m_s: 0.0000
    vel_d_m_s: 0.0000
    cog_rad: 0.0000
    timestamp_time_relative: 0
    heading: nan
    heading_offset: 0.0000
    heading_accuracy: 0.0000
    automatic_gain_control: 3510
    fix_type: 0
    jamming_state: 0
    vel_ned_valid: False
    satellites_used: 0

@davids5
Copy link
Member Author

davids5 commented Jan 23, 2022

On the FMU Side:

nsh> uavcan status
Pool allocator status:
        Capacity hard/soft: 500/250 blocks
        Reserved:  89 blocks
        Allocated: 21 blocks

UAVCAN node status:
        Internal failures: 0
        Transfer errors:   0
        RX transfers:      1822
        TX transfers:      442

CAN1 status:
        HW errors: 0
        IO errors: 0
        RX frames: 3397
        TX frames: 503
CAN2 status:
        HW errors: 10340
        IO errors: 10764
        RX frames: 0
        TX frames: 74

ESC outputs:
INFO  [mixer_module] Param prefix: UAVCAN_EC
control latency: 0 events, 0us elapsed, 0.00us avg, min 0us max 0us 0.000us rms
INFO  [mixer_module] Mixer loaded: no
INFO  [mixer_module] Driver instance: 0
Channel Configuration:
Channel 0: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 1: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 2: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 3: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 4: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 5: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 6: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Channel 7: value: 0, failsafe: 0, disarmed: 65535, min: 1, max: 8191
Servo outputs:
INFO  [mixer_module] Param prefix: UAVCAN_SV
control latency: 0 events, 0us elapsed, 0.00us avg, min 0us max 0us 0.000us rms
INFO  [mixer_module] Mixer loaded: no
INFO  [mixer_module] Driver instance: 0
Channel Configuration:
Channel 0: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 1: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 2: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 3: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 4: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 5: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 6: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0
Channel 7: value: 0, failsafe: 0, disarmed: 0, min: 0, max: 0

Sensor 'gnss':
name: uavcan_gnss
channel 0: node id 125 --> instance 0
uavcan: gnss: rtcm pub: 0 events, 0.00us avg, min 0us max 0us 0.000us rms

Sensor 'mag':
name: uavcan_mag
channel 0: node id 125 --> instance 0

Online nodes (Node ID, Health, Mode):
         125 OK         OPERAT
         127 OK         OPERAT

uavcan: cycle time: 8538 events, 751842us elapsed, 88.06us avg, min 20us max 2798us 236.102us rms
uavcan: cycle interval: 8538 events, 1893.18us avg, min 29us max 5433us 1241.073us rms


nsh>  listener sensor_mag

TOPIC: sensor_mag 3 instances

Instance 0:
 sensor_mag
    timestamp: 108551629 (0.002812 seconds ago)
    timestamp_sample: 108551628 (1 us before timestamp)
    device_id: 8944899 (Type: 0x88, UAVCAN:0 (0x7D))
    x: 0.1333
    y: -0.1140
    z: 0.3852
    temperature: nan
    error_count: 0
    is_external: True


Instance 1:
 sensor_mag
    timestamp: 108557704 (0.001166 seconds ago)
    timestamp_sample: 108556776 (928 us before timestamp)
    device_id: 396825 (Type: 0x06, I2C:3 (0x0E))
    x: 0.0515
    y: 0.2659
    z: 0.4227
    temperature: nan
    error_count: 0
    is_external: False


Instance 2:
 sensor_mag
    timestamp: 108562029 (0.000558 seconds ago)
    timestamp_sample: 108560941 (1088 us before timestamp)
    device_id: 396809 (Type: 0x06, I2C:1 (0x0E))
    x: 0.1500
    y: 0.2401
    z: 0.6257
    temperature: nan
    error_count: 0
    is_external: True

nsh>  listener sensor_baro

TOPIC: sensor_baro
 sensor_baro
    timestamp: 108550458 (0.015236 seconds ago)
    timestamp_sample: 108550450 (8 us before timestamp)
    device_id: 3997730 (Type: 0x3D, SPI:4 (0x00))
    error_count: 0
    pressure: 1014.7400
    temperature: 48.7100
nsh>  listener sensor_gps

TOPIC: sensor_gps 2 instances

Instance 0:
 sensor_gps
    timestamp: 108459667 (0.110565 seconds ago)
    time_utc_usec: 0
    device_id: 8748291 (Type: 0x85, UAVCAN:0 (0x7D))
    lat: 0
    lon: 0
    alt: -17000
    alt_ellipsoid: 0
    s_variance_m_s: 999.0000
    c_variance_rad: nan
    eph: 255.9375
    epv: 255.9375
    hdop: 100.0000
    vdop: 100.0000
    noise_per_ms: 0
    jamming_indicator: 0
    vel_m_s: 0.0000
    vel_n_m_s: 0.0000
    vel_e_m_s: 0.0000
    vel_d_m_s: 0.0000
    cog_rad: 0.0000
    timestamp_time_relative: 0
    heading: nan
    heading_offset: nan
    heading_accuracy: nan
    automatic_gain_control: 0
    fix_type: 0
    jamming_state: 0
    vel_ned_valid: True
    satellites_used: 0


Instance 1:
 sensor_gps
    timestamp: 108458352 (0.196854 seconds ago)
    time_utc_usec: 0
    device_id: 11010053 (Type: 0xA8, SERIAL:0 (0x00))
    lat: 272713186
    lon: -835855496
    alt: 4486424
    alt_ellipsoid: 4460057
    s_variance_m_s: 38.9020
    c_variance_rad: 3.1416
    eph: 4294967.5000
    epv: 3750002.2500
    hdop: 99.9900
    vdop: 99.9900
    noise_per_ms: 96
    jamming_indicator: 63
    vel_m_s: 0.0000
    vel_n_m_s: 0.0000
    vel_e_m_s: 0.0000
    vel_d_m_s: 0.0000
    cog_rad: 0.0000
    timestamp_time_relative: 0
    heading: nan
    heading_offset: 0.0000
    heading_accuracy: 0.0000
    automatic_gain_control: 858
    fix_type: 0
    jamming_state: 0
    vel_ned_valid: False
    satellites_used: 0

So I do not see the Baro FMU side

But the cannode is publishing it

nsh> uavcannode status
Pool allocator status:
        Capacity hard/soft: 500/250 blocks
        Reserved:  18 blocks
        Allocated: 7 blocks

UAVCAN node status:
        Internal failures: 0
        Transfer errors:   0
        RX transfers:      40569
        TX transfers:      243217

CAN1 status:
        HW errors: 0
        IO errors: 0
        RX frames: 55874
        TX frames: 382384

Publishers:
        sensor_mag -> uavcan.equipment.ahrs.MagneticFieldStrength2:1002
        sensor_baro -> uavcan.equipment.air_data.StaticPressure:1028
        sensor_baro -> uavcan.equipment.air_data.StaticTemperature:1029
        sensor_gps -> uavcan.equipment.gnss.Fix2:1063

Subscribers:
        uavcan.equipment.indication.BeepCommand:1080 -> tune_control
        uavcan.equipment.indication.LightsCommand:1081 -> led_control

uavcannode: cycle time: 839947 events, 61248498us elapsed, 72.92us avg, min 35us max 3286us 136.371us rms
uavcannode: cycle interval: 839947 events, 2300.75us avg, min 44us max 11153us 2779.689us rms

The Node Baro Temp data looks good here:

nsh>  listener sensor_baro

TOPIC: sensor_baro
 sensor_baro
    timestamp: 2319001215 (0.024507 seconds ago)
    timestamp_sample: 2319000273 (942 us before timestamp)
    device_id: 6845969 (Type: 0x68, I2C:2 (0x76))
    error_count: 0
    pressure: 1013.8383
    temperature: 44.9894

The UAVCAN Baro Temp data looks bogus: (unless this is a units thing)
image

@ryanjAA
Copy link
Contributor

ryanjAA commented Jan 23, 2022

So strange. I asked matek about if there were any revisions done to the board layout and there has not been since they came out with it so all hardware would (should) be the same.

You mag works but mine doesn’t which is odd. I’ll try fresh again (I downloaded the ci artifacts and loaded them but I will build your pr and try that instead and I did a full chip erase for good measure).

My gps looks the same as yours, it just doesn’t ever get sats unless I switch back to the other (ardu) setup.

If yours and @dirksavage88 works then mine is an outlier and I’ll get a new board to test but let’s see his output.

Also, strange about the Baro fc side. I’ll replicate the above.

@davids5
Copy link
Member Author

davids5 commented Jan 23, 2022

@ryanjAA @dirksavage88 - So on my MaTek board the sensors appear to be all working and publishing reasonable data.

I will have a look at the datapath on the baro end-to-end.

The GPS - node and FMU do not agree on the data.
Node:
image
FMU:
image

I believe the prams are working and now persist.

@dirksavage88
Copy link
Contributor

dirksavage88 commented Jan 23, 2022

Here is what I have:

Everything seems to line up on the can node nuttx side. The GPS sat count is still low, but it's at least showing a few sats. On the uavcan side I do not see accel or gyro publisher messages but that does not look like it's set up in PX4 CANNODE yet, at least I do not see accel/gyro pub in src/drivers/uavcannode/Publishers/RawAirData.hpp or any other publisher file.

Baro temp could also be some calculation on PX4 cannode because the nuttx side reads a valid temp. So the only thing that could be board (or GPS driver) related is the no GPS/low GPS sats....everything else could be addressed in PX4 CANNODE updates?

Screenshot from 2022-01-23 10-18-12

Screenshot from 2022-01-23 10-18-40

@ryanjAA
Copy link
Contributor

ryanjAA commented Jan 23, 2022

And your mag works too.

So my gps and mag are weird. I’ll get another and try. This is a brand new board but I bought it about a year ago. Just never used it.

@dirksavage88
Copy link
Contributor

@davids5 with the board level stuff taken care of, I am not sure how much else we can do from this side. I'm pretty comfortable with what works on the board at this point. At a later time I can look at can node publishers to better integrate some of the features on this board.

@davids5
Copy link
Member Author

davids5 commented Jan 25, 2022

@ryanjAA @dirksavage88 - I will look at the potential for RFI jamming the GPS and if I can improve it. I will also check the baro conversion.

@ryanjAA
Copy link
Contributor

ryanjAA commented Jan 25, 2022

Thanks @davids5. Hopefully the baro temp error is just a conversion (looks like it since it’s reporting correctly directly from the unit).

I spoke with Matek and did some troubleshooting with them. My board works if you press (really hard) on the mag so the they think a dry solder joint. Makes sense so I have a new board already on the way.

As for the gps, matek told me there is a bug in the latest ap_periph firmware that can lock the gps up and you need to reset it from u-center. Mine was running latest (ap_periph) although not sure how you can lock it from the firmware side tbh.

That said, you need to load inav on there to do uart passthrough to get access to the gps in ucenter. Specifically in the inav application (with the inav firmware loaded), go to the cli and type serialpassthrough 3 115200 Then ucenter will connect to it. At that point you need to go to the CFG settings and revert to default the BBR and Flash. That fixes the gps (see attached image) and @dirksavage88 hopefully that clears up your gps issues). When the next couple of these boards arrive this week I’ll report back findings.

48B3B9F5-3282-43E0-AA57-248493A716B1

@dirksavage88
Copy link
Contributor

@ryanjAA i couldn’t get the pass through to work. I am not sure if it’s my com port drivers or maybe I need an ftdi? I wonder if there is a way to do that revert method manually through the gps driver? That avoids having to flash inav, download ucenter, etc.

good news on the mag!

@davids5
Copy link
Member Author

davids5 commented Jan 25, 2022

I will add a pass thru command....

@davids5
Copy link
Member Author

davids5 commented Jan 27, 2022

@ryanjAA @dirksavage88

Running u-center and doing a crtl-f9 will get you into passthru.
you will have to change the baudrate on u-center to 38400 after resetting the config

@ryanjAA
Copy link
Contributor

ryanjAA commented Jan 28, 2022

@davids5 awesome and thanks! Will try this.

@davids5
Copy link
Member Author

davids5 commented Jan 28, 2022

@ryanjAA , @dirksavage88 Baro temp issue resolved.

  1. param set UAVCAN_SUB_BARO 1
  2. code was wrong. See 9438f52 (the conversion factor is -)

@davids5
Copy link
Member Author

davids5 commented Jan 28, 2022

I think the GPS is all better with the settings reset.

nsh> listener sensor_gps

TOPIC: sensor_gps 2 instances

Instance 0:
 sensor_gps
    timestamp: 859872636 (0.070522 seconds ago)
    time_utc_usec: 1643411022999596
    device_id: 8748291 (Type: 0x85, UAVCAN:0 (0x7D))
    lat: 337698095
    lon: -1163504987
    alt: 80203
    alt_ellipsoid: 48782
    s_variance_m_s: 2.0098
    c_variance_rad: 0.6582
    eph: 3.0208
    epv: 4.2975
    hdop: 3.5898
    vdop: 3.5898
    noise_per_ms: 0
    jamming_indicator: 0
    vel_m_s: 1.7889
    vel_n_m_s: 0.1710
    vel_e_m_s: -1.7390
    vel_d_m_s: -0.3830
    cog_rad: -1.4728
    timestamp_time_relative: 0
    heading: nan
    heading_offset: nan
    heading_accuracy: nan
    automatic_gain_control: 0
    fix_type: 3
    jamming_state: 0
    vel_ned_valid: True
    satellites_used: 8


Instance 1:
 sensor_gps
    timestamp: 859900814 (0.048997 seconds ago)
    time_utc_usec: 1643411022999922
    device_id: 11010053 (Type: 0xA8, SERIAL:0 (0x00))
    lat: 337697499
    lon: -1163504864
    alt: 71250
    alt_ellipsoid: 39829
    s_variance_m_s: 0.6340
    c_variance_rad: 0.7343
    eph: 4.1660
    epv: 5.7980
    hdop: 1.0600
    vdop: 1.6100
    noise_per_ms: 95
    jamming_indicator: 29
    vel_m_s: 0.0740
    vel_n_m_s: -0.0350
    vel_e_m_s: 0.0660
    vel_d_m_s: 0.2040
    cog_rad: 5.7726
    timestamp_time_relative: 0
    heading: nan
    heading_offset: 0.0000
    heading_accuracy: 0.0000
    automatic_gain_control: 1326
    fix_type: 3
    jamming_state: 0
    vel_ned_valid: True
    satellites_used: 9

@davids5
Copy link
Member Author

davids5 commented Jan 28, 2022

@dagar - I have upstreamed the NuttX changes. I will back port when they are ready.

The passthru commits (Nuttx dependent) and the UAVCAN node Baro Temp fix commits could be moved to separate PR or this can all come in at once. Let me know you preference.

@ryanjAA
Copy link
Contributor

ryanjAA commented Jan 29, 2022

@davids5 awesome! I have a new unit arriving tomorrow so I’ll give it a try on that.

The reset is a great add. I wonder if this can be done at the driver level to reset if needed for people that may run into this and don’t have windows for Ucenter. Just a thought.

Great on the temp/Baro fix. I was testing a pomegranate systems uavcan power module today and it was giving -400 degree constant (it monitors chip temp). I’ll take a look but I wonder if that is a similar thing to do the conversion error here.

@davids5
Copy link
Member Author

davids5 commented Jan 29, 2022

The reset is a great add. I wonder if this can be done at the driver level to reset if needed for people that may run into this and don’t have windows for Ucenter. Just a thought.

Yes that makes sense. I whish I knew exactly what to look for to detect the issue. If I reset the FLASH/RAM on every boot that could lead to degrading the FLASH. If anyone has some insight to the issue I would appreciate the input.

@davids5
Copy link
Member Author

davids5 commented Jan 31, 2022

@dagar - NuttX backports are merged to PX4 NuttX and this PR now points to the new tip.
This is ready to come in.

You and I should sort out the reason @PetervdPerk-NXP did not add boardconfig configs' to the build system. Once that is done we can re-distribute the board Kconfig for the passthru to the passthru Kconfig

@dagar
Copy link
Member

dagar commented Jan 31, 2022

How about a slightly more specific name like serial_passthru?

@ryanjAA
Copy link
Contributor

ryanjAA commented Feb 1, 2022

Tested. Everything works as intended and tried the passthrough as well. All works.

I believe the imu/accel is not on the node side (since if you enable it, it shows publishing data. I just don’t think it’s getting subscribed to) so that is px4 FC side.

But overall this is good to go 👍

@davids5
Copy link
Member Author

davids5 commented Feb 1, 2022

How about a slightly more specific name like serial_passthru?

I will rename it

dirksavage88 and others added 5 commits February 1, 2022 11:33
Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Added uavcan board identity

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Added usb.c, LED rework may be needed

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

PX4 dates added to all files

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Matek M9NF4 CAN Node initial board support

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Changed GPS to ttyS3 in board sensors, led board on/off definitions

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Added app descriptor section to canbootloader linker script

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Changed board naming convention to match vendor

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Changed canbootloader and nsh menuconfig

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Test defconfig changes, IRQ hardfault in bootloader

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

Working canbootloader, App firmware stil WIP

Working App firmware: changed romfsroot to 'cannode', TODO: verify GPS & IMU config

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>

TODO: Debug GPS no sats/low sats issue, no magnetometer on some boards

Signed-off-by: dirksavage88 <dirksavage88@gmail.com>
   SD is on SPI3 - correct pin mapping
   Fix DMA Mapping for all SPI and RX DMA on U[S]ART RX
   Fix Memory MAP SRAM size
   Removed unused GPIO
   Used proper I2C definitions
   Ensure Watchdog is configured for debugging
   Fixed FLASH param definitions
   Removed unedded SPI init

matek_gnss-m9n-f4:Correct Board ID and Size

Build order SJF

Added Support for F40x
   disable otg id
   cdcacm:support c_cflag in the termios structure
   and speed
   This addes the command serial_passthru which will pass data from one
   device to another. This can be used to use u-center connected to USB
   with a GPS on a serial port.

   Usage: serial_passthru [arguments...]
      -e <val>    External device path
                  values: <file:dev>
      -d <val>    Internal device path
                  values: <file:dev>
      [-b <val>]  Baudrate
                  default: 115200
      [-t]        Track the External devices baudrate on internal device

    With the -t option baudrate changes made on the PC connected to the USB
    will be set to the intrenal device.
@davids5
Copy link
Member Author

davids5 commented Feb 1, 2022

@dagar - renamed and re-tested

@dagar dagar merged commit 3358712 into master Feb 2, 2022
@dagar dagar deleted the pr-matek_m9nf4can branch February 2, 2022 02:49
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants