/**************************************************************************** * apps/examples/lely_master/master_main.c * * SPDX-License-Identifier: Apache-2.0 * * Licensed to the Apache Software Foundation (ASF) under one or more * contributor license agreements. See the NOTICE file distributed with * this work for additional information regarding copyright ownership. The * ASF licenses this file to you under the Apache License, Version 2.0 (the * "License"); you may not use this file except in compliance with the * License. You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the * License for the specific language governing permissions and limitations * under the License. * ****************************************************************************/ /**************************************************************************** * Included Files ****************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "sdev.h" #include "candev.h" /**************************************************************************** * Pre-processor Definitions ****************************************************************************/ #define SLAVE_ID 0x02 #define OBJ_COUNTER 0x2100 /**************************************************************************** * Private Types ****************************************************************************/ enum master_phase_e { PHASE_WAIT, /* wait for the slave, then read its info via SDO */ PHASE_SDO, /* SDO request in flight */ PHASE_RUN /* network operational, streaming PDO data */ }; struct co_net_s { can_net_t *net; co_dev_t *dev; co_nmt_t *nmt; }; /**************************************************************************** * Private Data ****************************************************************************/ static struct co_net_s g_net; static bool g_sdo_done; /**************************************************************************** * Private Functions ****************************************************************************/ /**************************************************************************** * Name: elapsed_ms ****************************************************************************/ static long elapsed_ms(const struct timespec *a, const struct timespec *b) { return (a->tv_sec - b->tv_sec) * 1000 + (a->tv_nsec - b->tv_nsec) / 1000000; } /**************************************************************************** * Name: on_can_send ****************************************************************************/ static int on_can_send(const struct can_msg *msg, void *data) { UNUSED(data); return comaster_candev_send(msg); } /**************************************************************************** * Name: nmt_start_remote ****************************************************************************/ static void nmt_start_remote(co_unsigned8_t id) { struct can_msg msg; /* NMT command frame: COB-ID 0x000, {command specifier, node-ID} */ memset(&msg, 0, sizeof(msg)); msg.len = 2; msg.data[0] = CO_NMT_CS_START; msg.data[1] = id; comaster_candev_send(&msg); } /**************************************************************************** * Name: on_sdo_up ****************************************************************************/ static void on_sdo_up(co_csdo_t *sdo, co_unsigned16_t idx, co_unsigned8_t subidx, co_unsigned32_t ac, const void *ptr, size_t n, void *data) { co_unsigned32_t val; UNUSED(sdo); UNUSED(idx); UNUSED(subidx); UNUSED(data); if (ac == 0 && n >= sizeof(co_unsigned32_t)) { memcpy(&val, ptr, sizeof(val)); printf("SDO: slave device type (0x1000) = 0x%08x\n", (unsigned)val); } else { printf("SDO: read failed, abort code 0x%08x\n", (unsigned)ac); } g_sdo_done = true; } /**************************************************************************** * Name: on_time ****************************************************************************/ static void on_time(co_time_t *time, const struct timespec *tp, void *data) { UNUSED(time); UNUSED(data); /* Update the wall clock */ clock_settime(CLOCK_REALTIME, tp); } /**************************************************************************** * Name: on_nmt_cs ****************************************************************************/ static void on_nmt_cs(co_nmt_t *nmt, co_unsigned8_t cs, void *data) { UNUSED(data); switch (cs) { case CO_NMT_CS_START: case CO_NMT_CS_ENTER_PREOP: { co_time_set_ind(co_nmt_get_time(nmt), &on_time, NULL); break; } case CO_NMT_CS_RESET_NODE: { exit(0); break; } case CO_NMT_CS_STOP: case CO_NMT_CS_RESET_COMM: default: { break; } } } /**************************************************************************** * Public Functions ****************************************************************************/ /**************************************************************************** * Name: main ****************************************************************************/ int main(int argc, FAR char *argv[]) { struct timespec now; struct can_msg msg; struct timespec t0; co_unsigned32_t last = 0xffffffff; int phase = PHASE_WAIT; int ret; co_csdo_t *csdo; co_unsigned32_t v; memset(&g_net, 0, sizeof(g_net)); g_sdo_done = false; setvbuf(stdout, NULL, _IONBF, 0); ret = comaster_candev_init(); if (ret < 0) { printf("ERROR: comaster_candev_init failed %d\n", ret); goto errout; } g_net.net = can_net_create(); if (!g_net.net) { printf("ERROR: can_net_create failed\n"); goto errout; } can_net_set_send_func(g_net.net, &on_can_send, NULL); clock_gettime(CLOCK_MONOTONIC, &now); can_net_set_time(g_net.net, &now); /* Create dev from struct */ g_net.dev = co_dev_create_from_sdev(&g_sdev_master); if (!g_net.dev) { printf("ERROR: co_dev_create_from_sdev failed\n"); goto errout; } g_net.nmt = co_nmt_create(g_net.net, g_net.dev); if (!g_net.nmt) { printf("ERROR: co_nmt_create failed\n"); goto errout; } /* Start the NMT service by resetting the node */ co_nmt_cs_ind(g_net.nmt, CO_NMT_CS_RESET_NODE); /* Set the NMT indication function after the initial reset */ co_nmt_set_cs_ind(g_net.nmt, &on_nmt_cs, NULL); printf("lely CANopen master running (node 0x%02x)\n", g_sdev_master.id); clock_gettime(CLOCK_MONOTONIC, &t0); while (1) { clock_gettime(CLOCK_MONOTONIC, &now); can_net_set_time(g_net.net, &now); while (comaster_candev_recv(&msg) > 0) { can_net_recv(g_net.net, &msg); } switch (phase) { /* Give the slave time to boot, then read its device type with * a Client-SDO transfer (works while both nodes are pre-op). */ case PHASE_WAIT: { if (elapsed_ms(&now, &t0) >= 500) { csdo = co_nmt_get_csdo(g_net.nmt, 1); if (csdo != NULL && co_csdo_up_req( csdo, 0x1000, 0x00, &on_sdo_up, NULL) == 0) { phase = PHASE_SDO; } else { t0 = now; } } break; } /* Once the SDO completes, bring the network to OPERATIONAL: * the master itself and the slave (NMT start). PDO/SYNC then * flow. */ case PHASE_SDO: { if (g_sdo_done) { co_nmt_cs_ind(g_net.nmt, CO_NMT_CS_START); nmt_start_remote(SLAVE_ID); printf("NMT: master + slave 0x%02x OPERATIONAL\n", SLAVE_ID); phase = PHASE_RUN; } break; } /* The slave's synchronous TPDO lands in object 0x2100 via the * RPDO mapping; print it whenever it changes. */ case PHASE_RUN: { v = co_dev_get_val_u32(g_net.dev, OBJ_COUNTER, 0x00); if (v != last) { printf("PDO rx: counter = %u\n", (unsigned)v); last = v; } break; } } usleep(1000); } errout: if (g_net.nmt) { co_nmt_destroy(g_net.nmt); } if (g_net.dev) { co_dev_destroy(g_net.dev); } if (g_net.net) { can_net_destroy(g_net.net); } return 0; }